NAVAL POSTGRADUATE SCHOOL 
Monterey, California 




THESIS 



DESIGN AND EVALUATION OF AN INTEGRATED, SELF- 
CONTAINED GPS/INS SHALLOW- WATER AUV 
NAVIGATION SYSTEM(SANS) 



by 

Randy G. Walker 
June, 1996 



Co- Advisors; Xiaoping Yun 

Robert B. McGhee 



Approved for public release; distribution is unlimited. 

'( 

Thesis 

W22215 




MOMTERSV CA 93943-5101 



REPORT DOCUMENTATION PAGE 


Form Approved 
0MB No. 0704-0188 


Public reporting burden for this collection of information is estimated to average 1 hour per response, including the time reviewing instructions, searching existing data sources 

gathering and maintaining the data needed, and completing and reviewing the collection of information Send comments regarding this burden estimate or any other aspect of this 

^fkctiun uf Wifon.iatitrt, tfAikfdtrvg tet the buKtei'. Stu’rrJb*, Iw ttrtur.Viatesvi eind fiepurts, i215 Jetterson ' 

Davis Highway, Suite 1204, Arlington, VA 22202-4302, and to the Office of Management and Budget, Paperwork Reduction Project (0704-0188), Washington, DC 20503 


1. AGENCY USE ONLY (Leave Blank) |2. REPORT DATE I 

1 June, 1996 1 


3. REPORT TYPE AND DATES COVERED 

Master’s Thesis 


4. TITLE AND SUBTITLE 

DESIGN AND EVALUATION OF AN INTEGRATED, SELF- 
CONTAINED GPS/INS SHALLOW-WATER AUV NAVIGATION 
SYSTEM (SANS) 


5. FUNDING NUMBERS 


6. AUTHOR(S) 

WaUcer, RaniJy G. 


7. PERFORMING ORGANIZATION NAME(S) AND ADDRESS(ES) 

Naval Postgraduate School 
Monterey, CA 93943-5000 


8. performing ORGANIZATION 
REPORT NUMBER 


9. SPONSORING/ MONITORING AGENCY NAME(S) AND ADDRESS(ES) 


10. SPONSORING/ MONITORING 
AGENCY REPORT NUMBER 


11. SUPPLEMENTARY NOTES 

The views expressed in this thesis are those of the author and do not reflect the official policy or position 
of the Department of Defense or the United States Government. 


I2a. DISTRIBUTION / AVAILABIUTY STATEMENT 

Approved for public release; distribution is unlimited. 


12b. DISTRIBUTION CODE 


13. ABSTRACT (Maximum 200 words) 

The main problem addressed by this research is to find an alternative to the use of large and/or expensive equipment required 
by conventional navigation systems to accurately determine the position of an Autonomous Underwater Vehicle (AUV) during 
all phases of an underwater search or mapping mission. 

The approach taken was to advance an existing integrated navigation system prototype which combines Global Positioning 
System (GPS), Inertial Measurement Unit (IMU), water speed, and heading information using Kalman filtering techniques. 
The hardware and software architecture of the prototype system were advanced to a level such that it is completely self- 
contained in a relatively small, lightweight package capable of on-board processing of sensor data and outputting updated 
position fixes at a rate of 10 Hz; an improvement from the 5 Hz rate delivered by the prototype. The major changes to the 
preceding prototype implemented by this research were to install an on-board processor to locally process sensor outputs, and 
improve upon the analog filter and voltage regulation circuitry. 

Preliminary test results indicate the newly designed SANS provides a 100% performance improvement over the previous 
prototype. It now delivers a 10 Hz update rate, and increased accuracy due to the improved analog filter and the higher sampling 
rate provided by the processor. 


14. SUBJECT TERMS 

Autonomous Underwater Vehicles, GPS/INS integration, navigation, NFS 
AUV, Kalman filtering 


15. NUMBER OF PAGES 

179 

ii. PRICE' espe 


17. SECURITY CLASSIFICATION 
OF REPORT 

Unclassified 


18. SECURITY CLASSIFICATION 
OF THIS PAGE 

Unclassified 


19. SECURITY CLASSIFICATION 
OF ABSTRACT 

Unclassifted 


20. LIMITATION OF ABSTRACT I 

UL 1 



NSN 7540-01-280-5500 



1 



Standard Form 298 (Rev. 2-89) 

Prescribed by ANSI Std. 239-18 






I 



11 



Approved for public release; distribution unlimited. 



DESIGN AND EVALUATION OF AN INTEGRATED, SELF-CONTAINED GPS/ 
INS SHALLOW-WATER AUV NAVIGATION SYSTEM(SANS) 

Randy G. Walker 

Captain, United States Marine Corps 
B.S.E.E, San Diego State University, 1990 

Submitted in partial fulfillment of the 
requirements for the degrees of 

MASTER OF SCIENCE IN ELECTRICAL ENGINEERING 

and 

MASTER OF SCIENCE IN COMPUTER SCIENCE 

from the 



NAVAL POSTGRADUATE SCHOOL 
June 1996 



DUDLEY KMOX LiBRARY 
NAVAL POSTGRADUATE SCHOOL 
MONTEREY CA 93943-5101 

ABSTRACT 

The main problem addressed by this research is to find an alternative to the use of large 
and/or expensive equipment required by conventional navigation systems to accurately 
determine the position of an Autonomous Underwater Vehicle (AUV) during all phases of 
an underwater search or mapping mission. 

The approach taken was to advance an existing integrated navigation system prototype 
which combines Global Positioning System (GPS), Inertial Measurement Unit (IMU), 
water speed, and heading information using Kalman filtering techniques. The hardware 
and software architecture of the prototype system were advanced to a level such that it is 
completely self-contained in a relatively small, lightweight package capable of on-board 
processing of sensor data and outputting updated position fixes at a rate of 10 Hz; an 
improvement from the 5 Hz rate delivered by the prototype. The major changes to the 
preceding prototype implemented by this research were to install an on-board processor to 
locally process sensor outputs, and improve upon the analog filter and voltage regulation 
circuitry. 

Preliminary test results indicate the newly designed SANS provides a 100% 
performance improvement over the previous prototype. It now delivers a 10 Hz update rate, 
and increased accuracy due to the improved analog filter and the higher sampling rale 
provided by the processor. 
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I. INTRODUCTION 



A. BACKGROUND 

An Autonomous Underwater Vehicle (AUV) can be capable of numerous missions both overt 
and clandestine. Such vehicles have been used for inspection, mine countermeasures, survey, 
observation, etc. [Yuh 95]. Recent research trends in underwater robotics have emphasized 
minimizing the need for human interaction by increasing the autonomy of such vehicles. 

The NFS ‘Phoenix’ AUV is an experimental vehicle designed primarily for research in support 
of shallow-water mine countermeasures and coastal environmental monitoring [Healey 93, 95, 
Brutzman 96]. In [Kwak 93], an approach is described for determining the position of submerged 
detected objects by executing a “pop-up” maneuver to obtain a GPS fix, and then extrapolating this 
fix backwards to the submerged object location using recorded inertial data. As explained in [Kwak 
93], navigation accuracy during such a surfacing maneuver is strongly enhanced by the use of 
accurate depth information available from low-cost pressure cells. However, this form of “aided” 
inertial navigation [Brown 92], is not applicable to a surfaced AUV. Inertial navigation is not 
needed in circumstances where continuous reliable reception of GPS satellite signals is possible. 
However, this does not apply to AUVs, unless perhaps they are fitted with a mast to extend a GPS 
antenna above the effects of wave action. Such a mast is not an attractive option for military 
operations, and in any event may be mechanically difficult. 

In efforts to overcome the problem of intermittent GPS satellite tracking for surfaced (or 
cruising near the surface) AUV navigation, an experimental system, using a low-cost strapped- 
down inertial measurement unit (IMU), has been designed to enable inertial navigation between 
GPS fixes. This system is well suited for pop-up navigation, so finding a means of navigating near 
the surface provides a complete solution to the overall navigation problem associated with 
transiting an AUV to a shallow water work site, recording the position of detected submerged 
objects, and then returning to a recovery site where stored mission data can be uploaded [McGhee 
95]. 

Many of the missions of the Phoenix class of vehicles can be separated into two distinct 
phases: transit and search. After being launched from an aircraft, submarine, or surface vessel, such 
an AUV would conduct the transit phase of the mission in order to arrive at the search area. After 
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the search phase, the AUV would transit back to a recovery position. Neither of these transit phases 
require as high a degree of navigation accuracy as the search phase. Once established in the mission 
area, the Phoenix would enter the search phase which might include missions such as mine- 
hunting, mapping, or environmental data collection. Typically, the search phase would require 
more precise navigation which could be provided by more frequent GPS fixes or by using 
Differential GPS (DGPS) or post-processing, if available. Both mission phases may involve 
waypoint steering and obstacle avoidance. 

One of the most important and difficult aspects of an AUV mission is navigation. It is 
important that the navigation system be robust if the AUV is to be capable of a wide variety of 
missions. In order to achieve such robustness, the AUV should be capable of navigating with the 
Global Positioning System (GPS) and/or an Inertial Navigation System (INS). The GPS is capable 
of highly accurate positioning when the AUV is surfaced, while an INS can be used for submerged 
navigation. In order to ensure accurate navigation for the various missions, the GPS and INS 
components can be combined. A favorable analysis of this type of navigation system was 
conducted in [McKeon 92]. The hardware and software architecture required for a typical mapping 
scenario was evaluated in [Norton 94]. 

[Bachmann 95] made the architecture evaluated in [Norton 94] a reality, and subsequently 
developed the first working prototype of the proposed Shallow-water AUV Navigation System 
(SANS). With the prototype SANS having achieved favorable results in open-water, at-sea test 
trials, the research reported in this thesis advances the SANS to another level of maturity, making 
it now ready for direct application to a real-world AUV. 

B. RESEARCH QUESTIONS 

This thesis will examine the following research topics: 

- Evaluate the hardware and software architecture of the GPS/INS prototype SANS. 

- Evaluate the feasibility of an AUV accurately navigating from point to point using GPS/INS 
while conducting open-ocean transit. 

- Develop a hardware configuration which will enable the GPS/INS SANS to be housed in 
one small, self-contained package. 
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C. SCOPE, LIMITATIONS AND ASSUMPTIONS 



This thesis reports the findings of the fifth year of research in an ongoing research project. The 
scope of this thesis is to investigate the feasibility of an AUV navigating from point to point using 
a combination of GPS/INS. The requirements for a SANS described by [Kwak 93] which impact 
this project are: 

- Low power consumption. Operation from an appropriately sized external battery pack for 12 
hours is desirable. 

- Limited exposure time. The amount of time that the GPS antenna is exposed in the search 
phase should be as short as possible. Up to 30 seconds of exposure is allowed, but time between 
exposures should be maximized. 

- Maintain clandestine operation. The GPS antenna should present a very small cross section 
when exposed and should not extend more than a few inches above the surface of the water. 

- Maximize accuracy. During the search phase of the mission, system accuracy of 10 meters 
or better is required with postprocessing, both submerged and surfaced. 

- Total volume not to exceed 120 cubic inches. Elongated, streamlined packaging is preferred. 

For the purposes of this research, DGPS will be used as ground truth data (without 

postprocessing) for determining appropriate Kalman filter gains. However, some real-world 
scenarios will only utilize the noncorrected GPS signal for real-time mission navigation and may 
require further tuning of Kalman filter coefficients. 

D. ORGANIZATION OF THESIS 

The purpose of this thesis is to present the development of a system meeting the mission 
requirements of the SANS. The term AUV is understood to include any small underwater vehicle 
(including human divers) which can easily carry such a compact device. The term “towfish” refers 
to the test vehicle (depicted in Figure 2.2) used to evaluate the SANS during at-sea testing. 

Chapter II reviews previous work on this project as well as previous work on GPS navigation 
and AUV submerged navigation. 

Chapter III provides an evaluation of the prototype SANS in the form of both a time domain 
and frequency domain analysis of the IMU sensor output signals. 
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Chapter lY is a detailed description of the hardware currently in use for this portion of the 
project. 

Chapter V is a detailed description of the software changes, additions, and updates made to 
support this portion of the project. The chapter includes a description of the C++ code required for 
future towfish experiments. This description provides an explanation of the class and object 
hierarchy used, as well as an outline of the major functions that were added as a result of this 
research. 

Chapter VI is a description of the experiment design and analysis of the experimental results. 

Chapter VO presents the thesis conclusions and provides recommendations for future 
research. 



4 



II. SURVEY OF RELATED WORK 



A. INTRODUCTION 

Autonomous Underwater Vehicles (AUVs) have the potential to be used in an efficient and 
cost effective manner in a variety of missions involving military and non-military applications. 
One of the most important aspects of an AUV mission is the ability to navigate accurately. Many 
possible missions, such as mine-hunting, require a high degree of navigation accuracy. This chap- 
ter will discuss some of the solutions for navigating an AUV. 

In general there are two categories of navigation systems: those that are based on external 
signals and those that are based on sensors. External-signal-based navigation systems such as, 
Loran, Omega, and GPS are only able to determine position while the signal receiver is exposed 
to the signal. Loran and Omega are relatively inaccurate compared to GPS. While Loran covers 
almost the entire northern hemisphere, it has almost no coverage in the southern hemisphere 
[Bowditch 84]. GPS on the other hand is capable of world-wide coverage with a high degree of 
navigational accuracy. [Logsdon 92] 

Sensor-based navigation can be implemented as a self-contained unit which can be composed 
of various types of equipment such as IMUs, acoustic transponders, or geophysical map 
comparison. Each of these components has its disadvantages. Acoustic transponders must be pre- 
deployed at precisely known locations and may require costly maintenance. Geophysical map 
interrogation requires a precise bottom contour map previously stored in the AUV’s computer. 
IMU-based navigation is prone to sensor drift, which if left uncorrected, can become very large. 

B. GPS NAVIGATION 

The Navigation Satellite Timing and Ranging (NAVSTAR) Global Positioning System (GPS) 
is a space-based radio positioning, navigation and time-transfer system sponsored by the U.S. 
Department of Defense (DoD). It was originally intended to provide the military with precise nav- 
igation and timing capabilities [Parkinson 80]. The system is designed to provide 24-hour, aU- 
weather navigation by providing total earth coverage using 24 satellites in 22,200 km orbits that 

are inclined 55°, with 12 hour periods. The satellites broadcast two L-band frequencies: LI 
(1575.4 MHz) and L2 (1227.6 MHz). Navigation and system data, predicted satellite position 
(ephemeris) information, atmospheric propagation correction data, satellite clock error informa- 
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tion, and satellite health data are all superimposed on these two carrier frequencies [Logsdon 92, 
Wooden 85]. 

There are two different navigation services available from the GPS satellites depending on the 
type of receiver being used: the Standard Positioning Service (SPS) and the Precise Positioning 
Service (PPS). The SPS is achieved by receiving the LI carrier signal which is broadcast with an 
intentional inaccuracy called Selective Availability (SA). SA limits world-wide navigation to 100 
m horizontal accuracy with a 95% confidence level [Logsdon 92], The PPS is limited to U.S. and 
allied military, and specific non-military uses that are in the national interest. Access to PPS is 
restricted by use of special cryptographic equipment. PPS provides the highest stand alone 
accuracy: 15 m Spherical Error Probable (SEP), a velocity accuracy of 0.1 m/sec, and a timing 
accuracy of better than 100 nanoseconds [Logsdon 92, Wooden 85]. 

In order to take full advantage of GPS precision without having access to cryptographic 
equipment, civilian customers have determined a way to improve the accuracy of the SPS. The 
most common way to work around the inaccuracies of the SPS is Differential GPS (DGPS), which 
may be used in real-time or during post-processing. DGPS is a method which allows highly 
accurate information to be obtained from GPS without the cryptographic equipment required for 
access to the P-code of PPS. The idea behind DGPS is to survey a receiver at a stationary site, allow 
the stationary site to determine the difference between its actual position and its GPS position, and 
broadcast the pseudorange corrections to any DGPS capable receivers. Real-time differential 
processing can reduce the typical 100 m accuracy of the SPS to 2-4 m regardless of the status of 
SA [Logsdon 92]. In the case of post-processing, it is possible to have the AUV record the raw PPS 
or SPS GPS information for later comparison to a known geographical site. Precise post- 
processing procedures can be used to reconstruct extremely accurate positioning information, 
typically in the submeter range. Table 1 shows a comparison of expected GPS accuracies. 



POSITIONING SERVICE 


PPS (m) 


SPS (m) 


Non-Differential 


16 


100 


Differential 


2-4 


2-4 



Table 1: Expected RMS GPS accuracy levels [Logsdon 92] 
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As GPS technology has matured, the size and cost of GPS receivers has decreased drastically. 
Not only is miniaturization improving, but GPS receivers have maintained or increased in 
performance capability. Since as early as 1992, the GPS industry has been able to produce 
receivers that are essentially a single printed circuit board. [Souen 92] reports that the Furuno GPS 
receiver module LGN-72 is an eight-channel receiver implemented on a single printed circuit 
board measuring 100 mm x 70 mm x 20 mm and requiring only 2 W of power. 

There is currently however, a performance trade-off associated with the miniaturization of 
GPS receivers. Trimble currently offers the PC Card 110 GPS receiver in the form of a Personal 
Memory Card International (PCMCIA) interface. This credit card sized device simply slides into 
any laptop, most palmtops, or pen-based computer compliant with PCMCIA (release 2.0). This 
miniature GPS receiver is capable of tracking eight satellites using three channels. However, 
because it does not have an allocated channel for each of the eight satellites it’s capable of tracking, 
it does not use a continuous tracking scheme, which degrades its acquisition time performance. In 
order to reduce the size of the receiver, manufacturers are reducing the number of channels on the 
receiver. In this configuration, GPS receivers are called “sequencing” receivers [Logsdon 92]. 
Sequencing receivers utilize a time-sharing technique to “dwell” on each satellite for a brief 
interval before switching to the next satellite in the sequence. Sequencing receivers have a typical 
acquisition time of about 2 minutes. Continuous tracking GPS receivers have a typical acquisition 
time of about 30 seconds, however, they are less compact in size as they have more receiver 
channels. Given this trade-off relationship between size and performance, the choice of GPS 
receiver must be made with the particular application in mind. If the application is not so dynamic 
(i.e., mobile navigation), a sequencing receiver would offer an adequate compromise. However, 
if the application requires a short time to initial acquisition, the most viable option is the continuous 
tracking receiver. 

Given the level of miniaturization and performance along with its excellent accuracy, GPS is 
an obvious choice for AUV navigation. One manner of using GPS to locate an AUV is to place 
buoys with GPS receivers at appropriate locations. These buoys would translate the GPS signal and 
retransmit an underwater acoustic signal. The AUV would determine its position via ranging and 
position fixes to the buoys. [Youngberg 91] suggests that the GPS antenna, receiver, processing 
and control subsystem, acoustic transmitter, battery power, and homing beacon could all be 
contained in a buoy measuring 123 mm diameter x 910 mm long and weighing 5-15 kg. A 
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simulation which showed the feasibility of this approach is presented in [Leu 93]. The simulation 
consisted of several sonobuoys spaced one kilometer apart. Due to uncertainties in buoy position 
caused by wave action and variations in altitude, the study proposed the use of Kalman filtering 
techniques to combine the outputs of an accelerometer and DGPS to enhance accuracy. Each GPS 
buoy would essentially act as a GPS satellite and broadcast its position via spread spectrum signals 
used by the AUV for ranging. This technique would eliminate the requirement to predeploy a 
surveyed transponder field. 

Another possible method for using GPS to determine the AUV’s position is to physically 
mount the GPS antenna and receiver onboard the AUV. One major concern would be that the GPS 
receiver would be unable to acquire satellites in a timely manner suitable to the mission due to 
splash effects on the antenna. [Norton 94] describes both static and dynamic test results which 
show that a submersible system is able to meet the accuracy and time requirements of the mission 
while being splashed by wave wash. 

C. INERTIAL NAVIGATION 

Inertial navigation is basically a complex method of dead reckoning. In its purest form it 
involves no outside references to fix position. All position data is calculated relative to a known 
starting point. An inertial navigation system (INS) continuously measures three mutually orthog- 
onal acceleration components using accelerometers. These measurements are taken in short time 
increments and multiplied by elapsed time in order to determine an estimate of instantaneous 
velocity. The three-dimensional change in position can then be determined by integrating respec- 
tive velocities over time. [Bachmann 95] 

The primary drawback of any INS is the tendency for small sensor drift rates to accumulate 
errors over time. Without outside references for correction, these errors grow relentlessly and 
eventually lead to large errors in the estimated position. Highly accurate inertial navigation 
systems can be constructed, but they are large, costly, and complex [Tuohy 93]. Size alone makes 
them unacceptable candidates for the SANS. In order to meet the SANS requirements, a low-cost 
INS can be integrated with GPS. GPS will provide the INS with the periodic position fixes 
necessary to correct slowly building INS errors. 

The acceleration measurements required by an INS can be made by several types of IMUs. 
The.se can be divided into two fundamental categories: gimbaled and strapdown. Due to their large 
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size and power requirements, gimbaled systems are not suitable for the SANS. In a strapdown unit, 
three mutually orthogonal accelerometers and three angular rate sensors are mounted parallel to 
the three body axes of the vehicle. Changes in linear and rotational velocities are continuously 
measured. Strapdown systems are smaller and simpler than gimbaled systems, but necessitate 
much larger computational capabilities [Logsdon 92]. 

D. INTEGRATED GPS/INS NAVIGATION 

SPS could be used to adequately perform both the transit and search phases of an AUV mis- 
sion. During the transit phases, non-differential SPS and a magnetic compass would provide the 
primary source of navigation data. In order to utilize GPS as a meaningful correction to a low-cost 
INS system, periods between fixes during the transit phase must not exceed the time in which an 
AUV could travel a distance greater than the horizontal accuracy of SPS (100m). The mapping 
phases of an AUV mission would require the vehicle to maintain more accurate navigational pic- 
ture both submerged and on the surface. This would necessitate the use of periodic differentially 
corrected GPS information in order to keep the INS system accurate while submerged. This dif- 
ferential correction could be provided in real-time during overt missions along friendly shores, 
provided a DGPS reference signal is available, or during mission post-processing following a 
clandestine mission. 

Integration of GPS and INS into a single system can produce continuously accurate 
navigational information even when using relatively low-cost components. This integration not 
only allows periodic reinstallation of the INS to correct accumulated errors but can also (with the 
aid of Kalman filtering techniques) improve the performance of the INS between fixes. Filtering 
the acceleration data with additional sensor information such as water speed and heading will 
further improve the quality of the integrated system. Overall, an integrated system will provide 
improved reliability, smaller navigation errors and superior survivability [Logsdon 92]. 

Kalman filtering is a method of combining all available sensor data regardless of their 
precision to estimate the current posture of a vehicle [Cox 90]. The filter is actually a data- 
processing algorithm which minimizes the error of this estimate statistically using currently 
available sensor data and prior knowledge of system characteristics. Each piece of data is weighted 
based upon the expected accuracy of the measurement it represents. In a complementary filter, 
low-frequency data, which is trusted over the long term, and high-frequency data, which is trusted 
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only in the short term, are used to “complement” each other providing a much better estimate than 
either can alone [Brown 92]. 

[Bachmann 95] demonstrated the use of this complementary filter technique by combining the 
low-frequency data of the accelerometers and compass with high-frequency angular rate and 
heading information. Intermediate position results were obtained by integrating high-frequency 
water-speed data. GPS data was used to reinitialize the system each time a fix was obtained and 
develop an error bias, expressed as an apparent current, to correct the system between fixes. The 
concept of using the relatively inexpensive IMU with limited accuracy coupled with differentially- 
corrected GPS has proven to be a viable solution to the challenge of shallow-water AUV 
navigation [Bachmann 95]. 

E. AUV SUBMERGED NAVIGATION 

There are many techniques available for submerged navigation, including dead reckoning, 
inertial, electromagnetic and acoustic navigation. With acoustic navigation, time of arrival and 
direction of propagation of acoustic waves are the two principal measurements made. A wide 
variety of acoustic navigation systems have been developed for underwater vehicle use. They are 
typically divided into long, short, and ultrashort baseline systems (LBL, SBL, and USBL). All 
involve the use of an array of acoustic beacons or receivers whose positions must be known to an 
accuracy better than the desired vehicle localization accuracy [Tuohy 93]. Unfortunately, most 
acoustic navigation systems require major expeditions for their accurate set-up and periodic main- 
tenance. This makes them expensive and in many ways reduces the level of autonomy achievable 
by an AUV. Also, acoustic navigation methods are affected by changes in the speed of sound in 
the ocean and suffer from refraction and multipath propagation problems in restricted shallow 
water coastal and ice-covered areas [Tuohy 93]. 

There are various other ways of determining a vehicle’s velocity and position while 
submerged without the aid of external signals. An AUV could use Doppler sonar to determine 
velocity. Charge Coupled Device (CCD) cameras, laser scanning, or variations in the earth’s 
magnetic field can also aid in determining position [Bergem 93]. Position could also be estimated 
by the double integration of acceleration as sensed by an Inertial Measurement Unit (IMU). 

Unless an AUV has access to outside references, it will not be able to refer to external signals 
while submerged. If this is the case, then the system must rely on some sort of dead reckoning. 
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Modem dead reckoning systems typically use magnetic or gyroscopic heading sensors and a 
bottom or water-locked velocity sensor [Grose 92]. The main problem is that the presence of an 
ocean current will add a velocity component to the vehicle which is not detected by the speed log. 
In the vicinity of the shore, ocean currents can exceed two knots [Tuohy 93]. Using dead reckoning 
with currents which are relatively large in relation to the typical 4-6 knot speed of an AUV can 
produce extremely inaccurate results [Tuohy 93]. 

There are many techniques for measuring accelerations and angular rates. These include using 
ring laser and fiber optic gyros, rotating mass gyros, vibratory rate sensors, and high performance 
IMUs. Inertial grade IMUs typically contain three angular rate sensors, three precision linear 
accelerometers and a three-axis magnetometer. The acceleration measurements required by an 
Inertial Navigation System (INS) can be made by several t 5 q)es of IMUs. Again, these can be 
divided into two fundamental categories: gimbaled and strapdown. All of these sensors are subject 
to drift errors which relentlessly increase with time. High quality sensors are subject to less drift 
but can cost up to $100,000 [Tuohy 93], making them unattractive for small AUVs. 

[McKeon 92] proposes a combination of GPS and INS to allow an AUV to determine position 
information. While submerged, the AUV uses a low-cost inertial navigation system. However, 
when on the surface the vehicle has access to GPS information. GPS/INS information could be 
combined with a Kalman filter techniques to reduce the errors during the next dive sequence as 
simulated in [Nagengast 92] and demonstrated in [McGhee 95]. 

F. THE PREVIOUS PROTOTYPE 

[Bachmann 95] describes in detail the previous hardware and software configurations of the 
SANS. For the benefit of the reader. Figures 2.1, 2.2, and 2.3 are given again in this thesis to aid 
in discussing the previous prototype. Figure 2. 1 shows a block diagram of the hardware assem- 
bled for at-sea testing of the SANS system. As seen in Figure 2.1, the system relied on an exter- 
nal 386 computer for processing sensor data, a modem connection for transmitting data packets to 
the towing vessel, and a coax cable to receive GPS data from the towed vehicle GPS antenna. The 
output of the A/D converter was being fed to the data-logging computer via an RS-232 connec- 
tion, and the GPS and DGPS receivers were physically located on the towing vessel. The hard- 
ware configuration for this research has been changed considerably, and will be presented in a 
subsequent chapter. 
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Figure 2.1: Towfish Experiment Hardware Configuration 

[Bachmann 95] 
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Figure 2.2: SANS and Towfish Components [Bachmann 95] 
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Figure 2.3: SANS Software Objects and Data Flow [Bachmann 95] 

Figure 2.2 presents a photograph of the major components of the previous prototype. Figure 
2.3 shows the SANS (as previously configured) software objects and the types of data passed from 
one to another. In its current configuration, the INS object no longer receives data from the 
sampler object, but rather from processor registers. Again, changes to the SANS software 
configuration will be presented in more detail in a subsequent chapter of this thesis. 
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G. SUMMARY 

The above survey has shown that there are many ways to overcome the challenges associated 
with AUV navigation. The choices range from simple dead reckoning to systems which use 
acoustic information from floating or stationary transponders, to complex systems which use 
sophisticated IMUs and GPS receivers combined with Kalman filtering techniques. 

Many AUV missions could be accompUshed using an integrated navigation system combining 
GPS and INS. Similar systems in other applications have been demonstrated to have superior GPS 
signal acquisition and reacquisition performance whenever loss of lock occurs. This results in 
improved survivability in hostile environments and smaller navigation errors. This research 
continues an ongoing experimental study pertaining to the development of such a system and the 
associated problems. The current system under evaluation is of small physical size and relatively 
low cost. The IMU selected is representative and has limited accuracy, so additional water-speed 
and magnetic heading information is required. This means that accelerometers are used mainly to 
derive low frequency attitude information, and are not utilized for velocity or position estimation 
over long periods. 

The availability of differential GPS in open-ocean tests in Monterey Bay will allow the 
experimental choice of navigation filter gains to accurately assess overall system performance in 
a variety of sea states and for various operational scenarios. Previous research on the protot}q)e 
SANS has produced test results and qualitative error estimates which indicate that submerged 
navigation accuracy comparable to GPS surface navigation is attainable. The research goal of this 
thesis is to refine the error estimates and the hardware configuration to allow more prolonged 
submerged navigation, and develop the SANS into a self contained system capable of being 
internally or externally attached to any AUV and delivering regular, accurate position updates. 
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m. EVALUATION OF THE PROTOTYPE SANS HARDWARE 



A. INTRODUCTION 

At the heart of the prototype SANS are the GPS receiver, the IMU, and other sensor devices. 
The GPS receiver, compass, and water speed sensor have not, up to this point in the course of this 
research, presented any serious problems in accuracy or dependability. However, the IMU out- 
puts, after being fed through signal processing and conditioning circuitry, are suspected sources of 
navigation inaccuracies [Bachmann 95]. 

This chapter does not provide any evaluation of the GPS/DGPS receivers, water speed sensor, 
or compass since there is no apparent major error contributions from these devices. It does, how- 
ever, provide an investigation into what noise is present in the real-world Systron-Donner 
MotionPak EMU. It also provides an evaluation of the low-pass filtering and conditioning cir- 
cuitry. Finally, it presents an evaluation summary of the prototj^e SANS hardware. 

B. NOISE CHARACTERISTICS OF THE MOTIONPAK INERTIAL SENSORS UNIT 

The Systron-Donner Model MP-GCCCQAAB-100 “MotionPak” inertial sensor unit consists 

of a cluster of three accelerometers and three “Gyrochip” angular rate sensors. The accelerome- 
ter specifications are shown in Table 3.1. The angular-rate sensor specifications are given in Table 
3.2. 

With the IMU placed on a stable test bench, polariod photographs were taken of an oscillo- 
scope screen display of each sensor output. Figure 3.1 depicts the respective noise characteristics 
in each of the EMU sensor outputs. This analysis gives hints there is broadband, “white” noise in 
the accelerometer sensors, while there is a 275 Hz sinusoidal signal present in the angular-rate 
sensors. Figures 3.2 and 3.3 depict the results of a power spectrum analysis of the accelerometer 
and angular-rate sensor outputs. Figure 3.2 shows the power spectrum between 8 Hz and 100 
KHz as well as between 8 Hz and 50 Hz. The spectrum analyzer used for these tests has its own 
power spectmm which was sufficiently below the sensor signals by 8 Hz. Thus 8 Hz was chosen 
as the start frequency. As these plots show, with a reference level of -20 dB (the reference level is 
depicted as the top-most horizontal index line), and using the value of 10 db/Div, the signal has a 
maximum value of -60 dB below 50 Hz. The signal energy drops off above this point and 
becomes relatively flat in the vicinity of 100 KHz. Figure 3.2 shows that the noise in the acceler- 
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Parameter 


Unit 


x-axis 


y-axis 


z-axis 


Range 


g 


1 


1 


2 


Scale Factor 


V/g 


7.469 


7.478 


3.727 


Scale Factor Temp. 
Coefficient 


%/deg C 


0.001 


-0.002 


-0.001 


Bias 


mg 


-2.447 


4.570 


-0.586 


Bias Temp. Coefficient 


Hg/degC 


-47 


-66 


-48 


Sensitivity 


Pg 


10 


10 


10 


Bandwidth 


Hz 


797 


757 


901 


Output Impedance 


Ohms 


2464 


2494 


1177 



Table 3.1: MotionPak Accelerometer Sensor Specifications [Systron-Donner 94] 



Parameter 


Unit 


x-axis 


y-axis 


z-axis 


Range 


deg/sec 


50 


50 


50 


Scale Factor 


mV/deg/sec 


50.151 


49.906 


50.242 


Scale Factor Temp. 
Coefficient 


%/deg C 


0.03 


0.03 


0.03 


Bias 


deg/sec 


-0.06 


0.23 


0.23 


Bias Temp. Coefficient 


deg/sec P-P 


3 


3 


3 


Sensitivity 


deg/sec 


0.002 


0.002 


0.002 


Alignment 


degrees 


0.26 


0.41 


0.34 


Noise 


deg/sec/A//7z 


0.008 


0.009 


0.008 


Bandwidth (to -90 deg 
phase) 


Hz 


70 


71 


71 



Table 3.2: MotionPak Angular-Rate Sensor Specifications [Systron-Donner 94] 



18 





Y-accelerometer Y-angular-rate 




Z-accelerometer Z- angular-rate 

Figure 3.1: Systron-Donner IMU Sensor Noise Characteristics 
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ometers is in fact broadband white noise, since it shows a power spectrum with energy across a 
wide range of frequencies. The white noise is most likely thermal and low-level EMI noise. Fig- 
ure 3.3 shows there is more “color” in the angular-rate sensor noise. Mainly, there appears to be 
harmonic noise between 1.5 KHz and 300 KHz and several high-power noise peaks at 100, 200, 
and 275 Hz. The peak at 275 Hz is most likely harmonic noise from the fundamental tuning-fork 
in the rate sensors. [Matthews 95] describes an investigation of signal noise in this same Systron- 
Donner MotionPak IMU. [Matthews 95] found a strong peak at 275 Hz, and attributed this noise 
to the internal tuning-fork oscillator in the sensor itself. 

The accelerometer sensor outputs depicted in Figure 3.1 show a DC bias of -21 mV and 125 
mV respectively. These DC biases are most likely caused by a combination of electronic sensor 
bias and slight tilts in the test bench surface. For the purpose of this evaluation, these DC biases 
are ignored. Furthermore, the software as described in [Bachmann 95], uses bias correction fac- 
tors that essentially negate any sensor bias affects on navigation accuracy. 




Figure 3.2: Power Spectrum of Accelerometer Sensors 
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Figure 3.3: Power Spectrum of Angular-rate Sensors 



C. THE SIGNAL PROCCESSING AND CONDITIONING CIRCUITRY 

The signal processing and conditioning circuitry, described in detail in [Schubert 95], low-pass 
filters the IMU outputs at a cutoff frequency of 10 Hz using an active, anti-aliasing, two-pole 
Sallen-Key Bessel filter design. This circuit further converts the dual-ended output swing of the 
IMU to a single-ended 0-5 volts. 



Figure 3.4 depicts the frequency response of the low-pass filter. Generally, this analysis con- 
firms that the -3dB frequency is about where it’s supposed to be, and the response does in fact 
rolloff at the expected -40dB/decade. 




Figure 3.4: Frequency Response of the Low-pass Filter 
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For the SANS application, in order for any sensor noise to not degrade the accuracy or resolu- 
tion of the measured signal, the energy in the filtered signal above 10 Hz must be attenuated down 
below: 

20 log 4^ = -lldB 
2 

As seen in Figure 3.4, the low-pass filter is not successfully able to attenuate signal noise 
energy in the stop-band below approximately 1 KHz (the center horizontal index line is at -70 
dB). The results of this filter short-fall can be seen by looking at the power spectrum of the accel- 
erometer and angular-rate sensor outputs after low-pass filtering. Figure 3.5 and Figure 3.6 depict 
the power spectrum of the filtered accelerometer and angular-rate sensors respectively. 




Figure 3.5: Power Spectrum of the Filtered Accelerometer Sensor Output 




Figure 3.6: Power Spectrum of the Filtered Angular-rate Sensor Output 
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This short-fall is more apparent in the angular-rate sensor outputs than those of the filtered 
accelerometer outputs. The low-pass filter appears to be doing an acceptable job in attenuating 
the noise energy in the accelerometers. However, the low-pass filter is not fully attenuating the 
noise energy in the angular-rate sensors to a level below the resolution of the least significant bit 
(LSB). Specifically, there are noise peaks at 30, 100, and 275 Hz that reach -60dB. Clearly, these 
high energy noise peaks are affecting the LSB. By noticing that these noise peaks represent a 
12dB over-power above the required noise floor, and solving the following equation for N: 

201og^ = -\ldB 
N = 1.99 

it’s apparent that this noise energy is actually affecting the two least significant bits of the mea- 
sured angular-rate signal. 

During system development, it was discovered that the x-angular-rate signal being input to the 
A/D had apparent slow growth behavior. Figure 3.7 shows the results of sampling the x-angular- 
rate sensor once every 60 seconds for a period of 4 hours after starting the SANS from an initially 
“cold” state. 




Seconds 

Figure 3.7: Sampled X- Angular-Rate Over a 4-Hour Period 
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The system hardware configuration used to collect the data shown in Figure 3.7 included the 
the LPF circuitry described in [Bachmann 95]. This growth in the signal turned out to be a signif- 
icant discovery possibly explaining some of the inaccuracy the SANS had been experiencing dur- 
ing previous testing. It was at first decided that this growth was due to heating effects in the IMU. 
However, after replacing the LPF circuitry with a newly designed circuit using commercially pro- 
vided filters (this new filter circuit is described later in Chapter IV), the problem went away. 
Therefore, the growth error in the x-angular-rate signal was attributed to heating effects on the cir- 
cuit components conditioning the x-angular-rate signal in the LPF circuitry used in the prototype 
SANS. 

D. SUMMARY 

This analysis lends further explanation to the results obtained in [McGhee 95], which shows 
the angular-rate output fluctuating as much as 4.88 mV; the three least significant bits of the 12-bit 
measured signal. Some of this fluctuation could be attributed to any of the causes stated in 
[McGhee 95], but this analysis gives qualitative evidence that the two least significant bits of the 
measured angular-rate data were in error due to sensor noise. 
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IV. SYSTEM HARDWARE CONFIGURATION 



A. INTRODUCTION 

Figure 4. 1 presents a block diagram for the hardware making up the redesigned SANS. Figure 
4.2 presents a photograph of the SANS components fully assembled into their testing configura- 
tion. The project box in which the components are currently mounted is not intended to be the 
final resting place for these components. A more permanent, water-tight, streamlined housing is 
currently in development. In fact, by using this particular off-the-shelf project box, and by itera- 
tively installing, testing, changing, and then reinstalling the components, an optimum space-effi- 
cient configuration has been achieved, which, in turn, has driven the design of the final housing. 

This configuration is significantly different from that presented in [Bachmann 95] for the pre- 
vious prototype. Mainly, the SANS components are no longer separated; all its components are 
physically located in one, self-contained package. In fact, when joined with its accompanying 
power source (a 12 VDC battery), it is capable of being strapped-down to a testing turntable, or 
inserted into the towfish (with slight towfish modifications) described in [Bachmann 95]. In its 
current configuration, the SANS has its processor and GPS/DGPS components “onboard,” thus 
no longer requiring the transfer of sensor data via modem to an external processor or GPS/DGPS 
receiver as was shown in [Bachmann 95]. In order to maintain the ability for human monitoring 
and interaction during the course of an experiment, the SANS’s processor is linked with an exter- 
nal processor via a DOS TCP/IP network environment. This external processor’s only function is 
to maintain a remote control session with the SANS processor and receive its attitude and position 
updates. Contrary to the original SANS proof of concept design presented in [Bachmann 95], the 
SANS now maintains the capability to on-board-process its own data in order to maintain its gen- 
eral capability to interface with any other higher-level processor (via a network), regardless of the 
application. 

B. HARDWARE DESCRIPTION 

I. Computer 

The on-board processor is an Extremely Small Package (E.S.P.) Cyrix 486SLC DX2 50 MHz 
computer. This computer, pictured in Figure 4.3, is specifically designed to offer off-the-shelf 
PC-compatible solutions in space and/or power constrained environments. Together, the E.S.P. 
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Figure 4.1: Block Diagram of the Redesigned SANS Hardware Configuration 



Processor Module and its accompanying modules provide a small, low-power system that does 
not sacrifice system performance compared to a standard, desk-top type system [MAXUS 95]. 
This particular E.S.P computer possesses a total of eight modules which perform various system 
tasks. A brief description of the tasks these various modules perform in the SANS is as follows: 

a. 486SLC DX2 CPU Module 

Besides providing the processing capability, the CPU Module provides the interface for a 
standard keyboard, the Flash PROM containing the system BIOS, and memory and bus controller 
logic [MAXUS 95]. 
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Figure 4.2: SANS Hardware Configuration 
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Figure 4.3: E.S.P. 486SLC DX2 50 MHz Computer 



b. DC-DC Power Module 

Provides for all the system power requirements up to a maximum 35W total output. It 
accepts an unregulated 12 VDC and provides the required +5, +12, -12, and -28 VDC to power 
various system components and optional peripherals (i.e., external floppy/hard drive). [MAXUS 
95] 



c. VGA Adapter Module 

Provides the interface to operate an external VGA monitor. 

d. PC I/O Module 

Provides for 2-Serial ports and 1 -Parallel I/O port. 

e. PCMCIA Module 

Provides two type-III PCMCIA sockets which conform to PCMCIA Release 2.01 standard 
[MAXUS 95]. These two ports can be used for a variety of compatible devices (i.e., Ethernet 
Adapter, Modem, GPS Receiver, etc.). For instance, this module can accept SRAM cards which 
can contain bootable system files or simply be used to provide storage media. This module was 
included in the current design to provide additional secondary storage in the form of PCMCIA 
SRAM cards, as well as to enable future expandability. 
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f. Ethernet Module 

Provides the SANS with an external ethernet interface. 

g. Analog to Digital (A/D) Module 

Provides 8 differential or 16 single-ended input channels at 12-bit resolution. It features a 
single-channel maximum sampling rate of 333 KHz, and an input range from +/- 1.25mV to 4 -/- 
lOV [MAXUS 95]. The A/D module provides a 34-pin external connector (J3) to which develop- 
ers can connect their input signals. In its current configuration, the A/D module samples only 8 of 
the available 16 single-ended channels. Table 4.1 shows the current pinout of connector J3 on 
the A/D connector board. 



Signal 


Pin 


Pin 


Signal 


GND 


1 


2 


GND 


GND 


3 


4 


IN_8B unused 


GND 


5 


6 


IN_8A depth 


GND 


7 


8 


EN_7B unused 


GND 


9 


10 


EN_7A water speed 


GND 


11 


12 


IN_6B unused 


GND 


13 


14 


IN_6A z-axis angular-rate 


GND 


15 


16 


IN_5B unused 


GND 


17 


18 


IN_5 A y-axis angular-rate 


GND 


19 


20 


EN_4B unused 


GND 


21 


22 


IN_4 A x-axis angular-rate 


GND 


23 


24 


IN_3B unused 


GND 


25 


26 


IN_3 A z-axis acceleration 


GND 


27 


28 


IN_2B unused 


GND 


29 


30 


EN_2 A y-axis acceleration 


GND 


31 


32 


EN_1B unused 


GND 


33 


34 


IN_1 A x-axis acceleration 



Table 4.1: Connector J3 Pinout 
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h. DRAM Module 

Provides for high-speed (70ns) memory storage available in 2, 4, 6, 8,or 16MB capacities 
[MAXUS 95]. This module is to the E.S.P. as a hard disk is to a standard desk-top PC. 

2. Inertial Measuring Unit 

The inertial navigation component of the SANS is provided by a Systron-Donner Model MP- 
GCCCQAAB-100 “MotionPak” inertial sensing unit, pictured in Figure 4.4. This self-contained 
unit provides analog measurements in three orthogonal axes of both acceleration and angular 
velocity. It consists of a cluster of three accelerometers and three “Gyrochip” angular rate sen- 
sors. General specifications are shown in Table 4.1. Accelerometer specifications and angular rate 
specifications are shown in Table 3.1 and Table 3.2 respectively. 




Figure 4.4: Systron-Donner Inertial Measuring Unit [Bachmann 95] 
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Parameter 


Units 


Range 


Input Voltage 


DC Volts 


+ 15,-15 


Input Current 


Amps 


+0.246, -0.196 


Temp. Range 


degrees C 


-40,80 


Weight 


grams 


912 


Temp. Sensor 


n A/deg k 


1.0 



Table 4.2: MotionPak General Specifications [Sytron-Donner 94] 

3. GPS/DGPS Receiver Pair 

The GPS/DGPS receiver used is the ONCORE 8-channel receiver which incorporates an 
imbedded DGPS capability [Oncore 95]. The receiver is capable of tracking up to eight satellites 
simultaneously. It can provide position accuracy of better than 25 meters Spherical Error Proba- 
ble (SEP) without Selective Availability (SA) and 100 meters (SEP) with SA. Typical Time-To- 
First-Fix (TTFF) is 18 seconds with a typical reacquisition time of 2.5 seconds [Oncore 95]. This 
receiver meets or exceeds the capabilities of the receiver described in [Norton 94], which demon- 
strated that under normal operating conditions, a receiver of this kind, is capable of meeting the 
accuracy and time requirements of the SANS project. [Norton 94] also demonstrated that a 
receiver with these qualities will perform well when using an antenna that is located on or near the 
sea surface as is necessary during a clandestine mission. Figure 4.5 shows the ONCORE GPS/ 
DGPS receiver used in the SANS project. 




Figure 4.5: ONCORE GPS/DGPS Receiver 
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4. Low-Pass Filters 

Based on the analysis given in Chapter III, the 2-pole Bessel filters were replaced with new fil- 
ters which offer a faster “rolloff’ in the stopband (now 80 dB/Dec). Each of the six IMU outputs 
is filtered by a 4-Pole, active, anti-aliasing low-pass Butterworth filter with a bandwidth of 10 Hz. 
The low-pass filters are model DP74 and are packaged in a standard 16-pin dual in-line package 
(DIP) [Frequency Devices 96]. These filters feature low-harmonic distortion, come factory tuned 
to a user-specified corner frequency, require no external components or adjustments, and operate 
with a dynamic input voltage range from non-critical +/- 5V to -1-/-18V power supplies [Frequency 
Devices 96]. To implement these filters into the SANS, a double-sided printed circuit board 
(PCB), shown in Figure 4.6, was designed and machined to receive all six filter DIPs as well as 
three quad op-amp LM324 DIPs configured as voltage-followers to provide input and output cir- 
cuit protection. Figure 4.7 shows the schematic diagram for one channel in this low-pass filter 
circuitry. 




Figure 4.6: The Double-Sided Low-Pass Filter PCB 




Figure 4.7: Schematic Diagram for One Channel of the Low-pass Filter Circuitry 
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5. DC-DC Converter 

To provide for the requisite +/-15 VDC, a DATEL model BWR-15/330-D12 DC-DC Con- 
verter is used to convert the unregulated 12 VDC battery input to regulated 4^7-15 VDC to power 
the low-pass filter circuits as well as the IMU. This converter features over-current and short-cir- 
cuit protection, a compact form-factor, high reliability, a minimum efficiency of 82%, and 
employs switching regulator technology, which minimizes heat generation and current usage 
[DATEL 95]. Though the DC-DC converter ensures a low noise/ripple in the output signal, the 
converter is augmented with additional capacitors in parallel with both the input and output pins 
of the device. Figure 4.8 shows the use of these capacitors in the DC-DC converter circuitry. 



Cl: 0. 1 uF Electrolytic Capacitor 
C2: 100 uF Electrolytic Capacitor 



Battery 



- Battery 



+15 V Out 



-15 V Out 



1 


C2 


[— ► 
■ci 


J 


[c^ 





Figure 4.8: DC-DC Converter Circuit 

The value of Cl was selected so as to filter any high-frequency radio frequencies (RF) that 
may get inducted into the circuit from surrounding components, and the value of C2 was selected 
so as to filter any high-ft'equency switching noise from the converter itself (the converter switches 
at 165 KHz) [DATEL 95]. This DC-DC Converter is physically mounted on the same PCB on 
which the low-pass filters are mounted. This configuration allows the use of PCB “tracks” to sup- 
ply the filter circuitry with +/-15VDC. This regulated +/-15VDC as well as IMU sensor signals 
are routed to and from the PCB via a DB25. 



6. Ribbon Cable 

Physically connecting the IMU, Low-pass Filters/DC-DC Converter PCB, the Analog-Digital 
Converter, input power, water speed sensor, and depth sensor, is a 25-strand flat ribbon cable. 
This cable enables all system components to be easily interconnected. Table 4.2 gives the pinout 
of this 25-strand ribbon cable as well as all DB25 connectors used in the system.. 
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Pin Number 


Color 


Description 


1 


Black 


IMU x-axis acceleration output 


2 


Gray 


x-axis acceleration input to A/D 


3 


Blue 


IMU y-axis acceleration output 


4 


Yellow 


IMU z-axis acceleration output 


5 


Red 


z-axis acceleration input to A/D 


6 


Black 


unused 


7 


Gray 


unused 


8 


Blue 


-15VDC 


9 


Yellow 


GND (Ground) 


10 


Red 


unused 


11 


Black 


+15VDC 


12 


Gray 


unused 


13 


Blue 


Depth sensor input to A/D 


14 


White 


y-axis acceleration input to A/D 


15 


Purple 


IMU z-axis angular-rate output 


16 


Green 


z-axis angular-rate input to A/D 


17 


Orange 


+12 VDC Battery In 


18 


Brown 


Water speed sensor input to A/D 


19 


White 


unused 


20 


Purple 


y-axis angular-rate input to A/D 


21 


Green 


x-axis angular-rate input to A/D 


22 


Orange 


IMU x-axis angular-rate output 


23 


Brown 


IMU y-axis angular-rate output 


24 


White 


unused 


25 


Purple 


unused 



Table 4.3: Pinout of the Ribbon Cable and DB25 Connectors 
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7. Compass 

The compass used in the SANS project is a Precision Navigation model TCM2 Electronic 
Compass Module. This compass does not employ the mechanical gimbal technology utilized in 
the compass described in [Bachmann 95], but rather employs a three-axis magnetometer and a 
high-performance two-axis tilt sensor in a small form-factor [TCM2 95]. The TCM2 compass 
provides readings of not only heading, but also pitch, role, and surrounding magnetic field 
strength. The TCM2 provides greater accuracy by calibrating (performed by the user) for distor- 
tion fields in all tilt orientations, providing an alarm when local magnetic anomalies are present, 
and giving out-of-range warnings when the unit is being tilted too far [TCM2 95]. 

8. Other Components 

The water speed sensor and the depth sensor are those described in [Bachmann 95] and there- 
fore not depicted in Figure 4.2. The GPS antenna shown in Figure 4.2 is an active antenna, which 
was selected for its performance and low-profile. Because the E.S.P. Ethernet module’s output 
media type is AUI, a standard AUI-to-BNC media converter is employed to allow the use of dura- 
ble RG-58 coax cable to span the roughly 100m distance required while pulling the towfish 
behind a towing vessel. The GPS/DGPS Interface box is nothing more than an adapter to inter- 
face the GPS receiver signal with the serial COM2 port of the E.S.P. computer. 

C. SUMMARY 

The SANS design described in this chapter is significantly different from that described in 
[Bachmann 95]. The processing capability, along with the GPS/DGPS receiver, is now on-board 
the SANS, making it completely self-contained with its only external link being that of a DOS 
ethemet environment to a remote PC. The IMU sensor data, after low-pass filtering, along with 
water speed and depth data, are converted firom analog to digital with 12-bit resolution, and then 
joined with GPS data in the on-board computer which computes updated attitude and position 
information to be exported over an ethemet socket. The hardware for this version of the SANS 
was chosen to comply with the requirements set forth in [Kwak 93]. Though there are many pos- 
sible choices of hardware for each of the components in Figure 4.2, trade-offs between accuracy, 
size, power requirements, and cost must be considered. As further advances in miniaturization are 
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made, accuracy will continue to increase while price and size decrease, thus making it easier to 
meet the challenges of the SANS baseline requirements. 
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V. SOFTWARE DEVELOPMENT 



A. INTRODUCTION 

The purpose of the SANS software remains the same as that described in [Bachmann 95]; “to 
utilize IMU, heading, and water-speed information to implement an INS, and then integrate this 
with GPS information into a single- system which can produce continually accurate navigational 
information in real time.” This chapter will not re-introduce the software already adequately pre- 
sented in [Bachmann 95], but rather will present only those changes, additions and updates, to the 
software described for use on the previous prototype SANS. 

B. SOFTWARE DESCRIPTION 

This implementation continues to use a majority of the software designed by [Bachmann 95], 
and for the most part, unchanged. However, the changes in the hardware architecture have driven 
subsequent changes to the software design to enable its use in the current SANS. Figure 5.1 
shows the software objects and data flow of the current SANS. Though already previously stated, 
these hardware changes, along with the subsequent software changes and additions, are again pre- 
sented in detail as follows: 

1. Compass Data 

In the SANS described by [Bachmann 95], the compass data was included in the packets 
received via modem from the towfish. This compass data was then parsed out of this packet for 
use. This Xmodem packet code is no longer required and has been removed. In the current ver- 
sion of the SANS, the compass data is received via the COM2 serial port, and thus requires code 
to communicate with the serial port, as well as code to check the “checksum” and header of each 
compass message received. This change was easily implemented by simply cutting, pasting, and 
altering previous com port and checksum code. This code is provided in Appendix A. 

2. GPS Data 

The code required to process GPS information is only slightly different from that described by 
[Bachmann 95]. The only changes required were driven by the use of an 8-channel receiver vice 
the 6-channel GPS receiver used previously. The 8-channel receiver sends a longer message, thus 
the only changes were to adjust the message length and the location of the checksum character in 
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the GPS message. 

The code was also modified to include a differential check. Before the code recognizes a GPS 
message as being valid, the message must pass three conditions; 1) A checksum check, 2) The fix 
must be based on at least 4 satellites, and 3) The differential bit in the message must be set (i.e., 
the fix must have the differential correction calculated into it). This updated code is provided in 
Appendix A as well. 




Figure 5.1: SANS Software Objects and Data Flow 
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3. Inertial Sensor Data 



Like the compass data, the inertial sensor data was previously included in the packets received 
via modem from the towfish. Currently however, the filtered inertial data is input directly to the 
A/D converter module in the on-board processor. Therefore, there is additional code running in 
the SANS software system which operates the A/D converter module, as well as buffers this data. 

a. A2D 

The A/D module, when delivered, came with C source code to run a demo of the module. 
This code was modified and converted to C-H- to fit the SANS application and its object oriented 
design. The A2D class provides all the requisite software operation of the A/D module in the 
E.S.P. computer. The A/D module is completely controlled through software. Primary to this 
control is the manipulation of the A2D Control Register and the A2D Status Register. These reg- 
isters are manipulated by writing to and reading from specific memory addresses. Specifically, 
the A2D Control Register is at address 0x108, and the A2D Status register is at 0x102. The A2D 
class is designed such that the user maintains some degree of flexibility. For instance, the user can 
choose between one of two base addresses, 0x100 or 0x300, from which these important A2D 
registers are then created by adding an offset (08h and 02h respectively) to this base address. 

Though the A2D class has many member functions, the SANS software only uses a few in 
accomplishing its mission. All those member functions not directly utilized in this particular 
application are useful to troubleshoot problems with the A2D module, or allow a wide range of 
options to tailor its use to a particular application. The class code is well commented, and easily 
stands without any further discussion. However, for the benefit of the reader, the following gen- 
eral discussion of how the A2D module works in the SANS application is deemed beneficial. 

12 

The A2D, as stated in Chapter IV, provides 12 bits of resolution, or 2 = 4096 discrete 

quantization levels. There are two modes to employ the A2D module: differential mode, or sin- 
gle-ended mode. The SANS application employs the A2D in the single-ended mode of operation. 
In this mode, the A2D is able to sample the dual-ended swing of the IMU sensor signals, and rep- 
resent these voltages as a digital value in the range 0 - 4095. A general A2D conversion table is 
provided as Table 5. 1 to further illustrate how the sensor voltages are mapped over to their digital 
equivalents. 
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Sensor DC Voltage 


Converted Equivalent 


-HlO Volts 


4095 


-1-5 Volts 


3071 


0 Volts 


2047 


-5 Volts 


1023 


-10 Volts 


0 



Table 5.1: A2D DC-to-Digital Conversion Mapping 

When an A2D object is instantiated, the class constructor (see Appendix A, A2D.cpp) sets 
several default data member values, and then reads the A2D configuration file A2D.cfg. This 
configuration file provides a simple manner in which a user can change the way the A2D module 
operates without having to re-compile the source code. When the function readConfigFile() is 
called, it reads the A2D.cfg file one line at a time and loads those respective variables described 
on each line of this file with those values found on each line. This prepares the software for ini- 
tializing the A2D hardware. The constructor initializes the system addresses to setup for A2D 
operation, then initializes the A2D hardware using those variables that were loaded upon reading 
the configuration file. The A2D object gets instantiated when a Sampler object gets instantiated 
as the A2D object “a2dl” is a private data member of the Sampler class. 

The A2D module gets set into operation by a call to initSampler(). This sampler class 
member function executes a sequence of function calls to A2D class member functions which set 
the A2D module into operation. Mainly, they program the sequencer to tell it which channels to 
sample in which order, reset the A2D First-In-First-Out (FIFO) to enable it to receive data, and 
then toggle the trigger bit in the A2D Control Register from a low to a high which starts the A2D 
into operation. 

During SANS operation, the Sampler class member function readSamples() is called 
repeatedly to retrieve inertial data from the A2D FIFO. It first checks to ensure that the FIFO is 
not FULL. If the FIFO ever gets filled without being immediately emptied, it will continue to 
push data into the FIFO. Since there is no room for additional data, all samples from that point on 
will be lost. So, it is evident that preventing the FIFO from overflowing is paramount to SANS 
operation. If the this check is ever true, the SANS software will terminate execution. To prevent 
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FIFO overflow, one need only be mindful of the rate at which the A2D is sampling its inputs and 
be sure to empty out the FIFO at the same rate or faster. If the FIFO does have data in it, this data 
is emptied from the FIFO and stored in a doubly-subscripted array with 8 rows and 1000 columns 
to coincide with storing up to 1000, 8 channel samples of sensor data. Figure 5.2 presents a 
model of this doubly-subscripted array and how it stores the data. 
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depth 


depth 




0 


1 . . 


999 



Sample Number/ Array Index 



Figure 5,2: Model of the A2D Sample Array 
As the samples are being emptied from the FIFO, the variable “timeCounter” is incre- 
mented once for every 8 samples that are pulled from the FIFO. This variable is then multiplied 
by the sample period to calculate the “deltaT’ or the time between adjacent samples. The reason- 
ing behind using this type of data structure to temporarily store the data is to enable access to a 
history of samples in order to employ a digital filtering scheme. In the case of the SANS, it 
employs a very simple form of low-pass filtering by averaging over all the samples received since 
the last sample was taken from this array. 

C. SUMMARY 

All software additions and updates to the SANS software were made in keeping with object- 
oriented paradigms. The software was written in Borland 3.1, C-H- for DOS. 

Since the publication of [Bachmann 95], there have been many significant as well as minor 
changes/updates to the SANS software. The compass data is now received from a serial port vice 
being received via an Xmodem packet. The GPS data is stUl received from a serial port, but the 
GPS receiver is now an 8-channel receiver instead of a 6-channel. The inertial, water speed, and 
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depth data is no longer being received via an Xmodem packet, but rather is being input directly 
into the A/D module in the E.S.P. computer. Consequently, all the Xmodem packet code is no 
longer used in the SANS software system. All these hardware changes have driven software 
changes that have propagated throughout the software. For this reason, a complete copy of all 
SANS software is given as Appendix A. A further discussion of those software updates incorpo- 
rated into the SANS between the publication of [Bachmann 95] and this thesis, is presented in 
[Bachmann 96]. 
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VI. SYSTEM TESTING 



A. INTRODUCTION 

This chapter presents the test method and the experimental results of testing used to determine 
the functionality and accuracy of the SANS. Bench testing was performed to ensure the entire 
system was functioning properly in its current state. The system was then tested by conducting 
tilt-table tests in order to verify the operation of the Inertial Measurement Unit and its associated 
filter software. It was further tested by conducting a static GPS test to test the operation of the 
positioning capabilities of the SANS. Lastly, it was given a static heading test to ensure proper 
operation of the Kalman filter in determining heading. 

Figure 6.1 presents a data flow diagram of the SANS navigation filter. This diagram is pre- 
sented at this point to provide a basis for discussion of the following test results. The reader is 
referred to [Bachmann 95, 96], and [McGhee 95] for an in-depth discussion of this filter. This 
twelve-state velocity-aided navigation filter is implemented in the SANS software provided as 
Appendix A and Appendix B. 

B. IMU TESTS 

The purpose of these tests was to qualitatively ensure that the SANS was able to accurately 
track changes in attitude. Paramount to getting the SANS to do this, is to find a suitable value for 
the gain Kl(shown in Figure 6.1), the biasWght, sampleWght (all in ENS.CPP), and x/yAc- 
celScale factors (in LOCATION.H). The results presented in this chapter are for the pitch axis 
only. A similar process still remains to be done for the roll axis. 

With the SANS mounted to the tUt-table described in [Bachmann 96], a series of pitch tests 
were conducted. For these tests, the IMU outputs were sampled at a rate of 40 Hz. During test- 
ing, it was noticed that the SANS produced update rates between 6-10 Hz. 

Tuning data for the SANS was obtained by moving the SANS unit through pitch excursions 
within a 45 degree range. The attitude as determined by the SANS was then plotted and compared 
with the actual motion of the unit. Through this comparison, it was possible to determine an initial 
scale factor. This process was repeated several times until the attitude determined by the filter 
‘matched’ the true motion of the unit. From this analysis, a rate sensor scale factor of 4.1 enabled 
the filter to register a pitch of 45 degrees when the SANS was pitched to that angle. 



43 




In order to tune the filter for accurate operation, the rate sensor bias value must first be deter- 
mined. To do this, the gain value K1 is set to zero in order to prevent accelerometer data from 
getting to the first integrator. Without the accelerometer data, only the high frequency data from 
the angular-rate sensors is input to the first integrator with the estimated bias. Taking the com- 
manded tilt-table angles to be truth then, any errors in attitude can be attributed to the bias and 
scale factor. 

Having determined the initial scale factor to be 4.1 and setting K 1=0.0, a series of pitch tests 
were conducted in order to determine the correct bias weights. In order to obtain a starting point 
for this analysis, and based on similar testing discussed in [Bachmann 95], an initial bias value 
(biasWght) of 0.999 was chosen for the first pitch test. Figure 6.2 shows the results of the first 45 
degree pitch excursion with K1 = 0.0, bias value = 0.999, and the scale factor set to 4.1. 
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Figure 6.2: 45 Degree Pitch Excursion w/Kl=0, biasWght=.999, scale factor= 4.1 



The shape of the plot in Figure 6.2 is determined by the value of a “virtual’ time constant. For 
the sake of this discussion, this time constant is T . The method in which the SANS software 
determines the rate bias value can be essentially modeled as a low-pass filter. Specifically, the 
SANS software figures this rate bias according to the model depicted in Figure 6.3. This figure 
describes a linear system where the accumulating rate bias is subtracted from each new angular- 

rate value, multiplied by the sampleWght ((At) /t ), which is (1 - biasWght), and then this 
value is added to the rate bias sum. 




Figure 6.3: Model of Rate Bias Low-pass Filter 
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From linear system theory, in the s-domain, it can be shown that the transfer function of this 



system is: 



1 + 

Assuming the 10 degree/second pitch excursion to be a unit step function, the unit step 
response of this system is: 



By taking the first derivative, it can be shown that the bias filter output slope is 1/x . Using the 
relation that the sampleWght (.001 for the first test) is equal to the ratio of the time between sam- 
ples (at a sample rate of 7 Hz, this gives a period of 0.142) and x , it can be shown that, for the first 
test depicted in Figure 6.2, x is approximately 142 seconds. That is. 



Given the known angular-rate of the pitch excursion was 10 degrees/second, the bias filter output 
slope can then be calculated as: 



From the plot, it appears to have taken about 4.5 seconds to complete the 45 degree angular-rate 
excursion. Multiplying the filter slope by the 4.5 seconds yields a resulting bias estimate of 
approximately 0.3 deg/sec. Again, fi-om the plot, the slope of the curve after it reached 45 deg, 
but before it started its return to zero, agrees with the calculations. 

In short, the filter is behaving just as it should. During the course of the excursion to 45 
degrees, the bias filter accumulates a rate bias of 0.3 deg/sec. Since the filter time constant is only 
142 seconds, and the SANS only underwent a 4.5 second excursion, it did not have time to correct 
for this bias. Additionally, since the bias is subtracted from each new sample, the result is the 
negative sloping portion of the curve while the SANS is at 45 degrees pitch. 

Obviously, the goal of this analysis is to minimize any accumulated rate bias while the SANS 
is undergoing pitch or roll excursions. To accomplish this, the value of the time constant x needs 
to be long enough to minimize the bias filter slope, which in turn, minimizes the accumulated rate 
bias. 
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Figure 6.4: 45 Degree Pitch Excursion w/Kl=0, biasWght=.9999, scale factor= 4.1 



Figure 6.4 shows the test results after increasing the filter time constant by a factor of 10. As 
shown in the plot, the pitch values sloped off less by about a factor of 10 (as expected) during a 
pitch excursion to 45 degrees. The decreasing curve leading up to the 45 degree pitch is due to 
not initializing the bias filter with enough samples. For these tests, the filter was only initialized 
with 100 samples, which represents about 15 seconds. However, since the time constant is much 
larger, the filter should be initialized over more samples. Ideally, the filter should be initialized for 
X seconds. The plot stiU shows some bias effect as well as the effects of undersampling (or slow 
update rate). Here, the navigation filter gets an update just before the tilt-table stops, but the filter 
still thinks it has a rate bias and applies it to the next sample, which occurs after the tilt-table has 
stopped. 

Figure 6.5 shows the result of another similar test, only with K1 = 0.1, biasWght = .9999, 
sampleWght = .0001, and xAccelScale = 4.1. This test re-introduces the low-frequency data 
(accelometer data) into the integrator. This plot shows yet more accurate performance, but dis- 
plays an “overshoot” in both directions of the excursion. This overshoot can be alleviated by 
adjusting the scale factor. 
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Figure 6.5: 45 Degree Pitch Excursion w/Kl=0.1, biasWght=.9999, scale factor= 4.1 
Figure 6.6 shows the result of another similar test, only with K1 =0.1, biasWght = .9999, 
sampleWght = .0001, and an adjusted scale factor, xAccelScale = 3.895. This plot shows an 
undershoot, indicating the scale factor was too low. Additionally, it still shows a small amount of 
sampling effect. This can only be corrected by getting the navigation filter to run faster. This can 
be easily accomplished by adding a math co-processor to the SANS computer. 




Figure 6.6: 45 Degree Pitch Excursion w/Kl=0.I, biasWght=.9999, scale factor= 3.895 
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C. STATIC GPS TEST 



The purpose of this test was to ensure proper functioning of the DGPS and the Kalman filter in 
calculating position. With the SANS stationary, operating, and receiving differentially corrected 
GPS fixes, position updates were recorded over a 30 minute period at a rate of 10 Hz. Figure 6.7 
confirms proper operation of the DGPS hardware and the Kalman filter software. The position 
updates show an oscillating behavior roughly centered about the origin. 
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Figure 6.7: Static GPS Test Results 

The results shown in Figure 6.7 are well within the 10 meter requirements outlined in [Kwak 
93]. By looking at a plot of the x position, it was determined that the greatest drifts shown in Fig- 
ure 6.7 occurred early into the test, and as time went on, these drifts got smaller. This plot shows 
how the SANS Kalman filter “learns” as time progresses. In fact, this drift becomes smaller and 
smaller due to the accumulation of apparent current. At the beginning of the test, the apparent 
current is small. Any error in location is attributed to apparent current, so as time goes on, the 
magnitude of the apparent current grows. Because the apparent current velocity is added to the 
North & East velocity, the difference between the INS determined position and the GPS deter- 
mined position decreases with time. Figure 6.8 shows a plot of the magnitude of the apparent cur- 
rent during the same test as that depicted in Figure 6.7. This plot shows that the apparent current 
starts out small, but quickly adjusts to a randomly varying value reflecting the accumulation of all 
system errors. 
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Figure 6.8: Apparent Current 



D. STATIC HEADING TEST 



The purpose of this test was to ensure that the heading calculated by the Kalman filter was in 
fact continuous in behavior. That is, a rotation through North in the clockwise direction should not 
cause the heading value to go back to zero, but rather continue to increase. With the SANS sta- 
tionary and running, the compass was rotated several complete rotations in both the clockwise 
and counterclockwise directions. As Figure 6.9 shows, the heading value is in fact continuous in 
nature as the plot of heading does not show any discontinuity at the zero degree crossing point. 
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Figure 6.9: Static Heading Test Results 



E. SUMMARY 

This chapter provides an explanation and results analysis of the experimental tests performed 
on the SANS to determine its proper functioning under the newly designed architecture. 

Dynamic tilt-table tests were performed in order to “tune” the software scale factors, bias val- 
ues, and gain factor K1 used in the Kalman filter. These test also confirmed the proper operation 
of the SANS in general. 

The DGPS and the associated software was tested by conducting a static test during which the 
SANS remained stationary whUe getting position fixes. This test confirmed the proper operation 
of the DGPS hardware and software. The behavior of the results also indicate that the SANS soft- 
ware Kalman filter is properly attributing its navigation errors to apparent current, as expected. 

The compass and associated software was tested by analyzing the heading output of the SANS 
while the compass was rotated through several complete rotations. The results show a continuous 
heading output from the SANS, that is, there are no discontinuous “jumps” as the compass rotates 
through North. 

Though extensive testing of the SANS still remains, initial testing indicates qualitatively that 
the SANS does function properly. Based on observations during this testing, the SANS shows an 
improved performance over the previous proof of concept prototype. Mainly, the SANS now pro- 
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vides an improved update rate of up to 10 Hz in comparison to the 5 Hz of the prototype. This 
increase in update rate has reduced the effects of undersampling experienced with the prototype 
and explained in [Bachmann 95]. Figure 6.6 provides insight to conclude the SANS is stiU suffer- 
ing from some effects of undersampling. This effect wiU most likely be alleviated with the 
planned addition of a math co-processor to the E.S.P. CPU Module. 
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VII. CONCLUSIONS AND RECOMMENDATIONS FOR FUTURE WORK 



A. CONCLUSIONS 

The research issues addressed by this thesis were 1) Evaluate the hardware architecture of the 
prototype SANS, 2) Develop a hardware configuration which will enable the SANS to be housed 
in one small, self-contained package, and 3) Evaluate the feasibility of an AUV accurately navi- 
gating from point to point in open-ocean transit using this hardware configuration. 

The work conducted in addressing the first of these questions revealed several sources of nav- 
igation inaccuracy. The analog low-pass filter circuitry used in the prototype SANS was not ade- 
quately attenuating the angular-rate sensor noise below the resolution of the least significant bit of 
the 12- bit A/D, and was over-designed to require high current, thus generated a great deal of heat. 
It was also discovered that circuit components were being adversely affected by heat. Specifi- 
cally, the x-angular-rate signal displayed a significant growth behavior over time as the circuit 
heated up. Both of these problems were alleviated by employing higher quality, commercially 
available analog filters, and also employing a switching DC-DC converter in place of the linear 
regulators employed in the prototype SANS. All this circuitry was designed into a small double- 
sided PCB. 

In addressing the second of these questions, an emphasis on making the SANS integrated and 
self-contained produced a configuration in which all SANS components are physically packaged 
in a project box measuring 17.5” X 6.125” X 3.0”. In the prototype SANS, the EMU, compass, 
GPS antenna, and water speed/depth sensors were physically separated from the computer and 
DGPS receiver. The newly designed SANS no longer requires the use of the data logging com- 
puter (which did the A/D conversion and assembled data packets) or the Motorola modems. The 
SANS now employs a E.S.P. 486 DX2 50 mini computer with an internal 12-bit A/D converter, 
DC-DC converter module (enables computer to be powered from a 12 Volt battery), Ethernet 
module (enables SANS to be networked in a client/server environment), PCMCIA module (pro- 
vides SANS with expandability), and memory storage modules. From observing the SANS oper- 
ation during bench testing, it is evident the system runs faster (gives faster update rates) since it is 
no longer hindered by the “bottleneck” induced by the Xmodem transfer of data employed by the 
prototype SANS. 
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Though the last research question was not directly addressed in detailed at-sea testing, those 
tests that were conducted for this thesis qualitatively indicate that the newly designed hardware 
configuration provides a higher level of performance to the SANS. From this result, it can be 
extrapolated that the feasibility of an AUV accurately navigating during open-ocean transit, is at 
least as high as that achieved with the prototype SANS. It is likely that the newly designed SANS 
will demonstrate increased accuracy in at-sea trials. 

B. RECOMMENDATIONS FOR FUTURE WORK 

There are many ways to build on the foundation this and related research has established. In 
addition to comprehensive at-sea testing, and since this project research is dynamic in nature, 
there are several other areas that remain to be addressed as they pertain to the SANS. Before any 
worthwhile at-sea testing can be accomplished, further tilt-table/bench testing needs to be con- 
ducted. In order to optimize the performance of the Kalman filter used for inertial navigation, 
testing needs to be done in order to better optimize the gains in the Kalman filter. By analyzing 
the data from varied accelerometer and angular-rate tests, one can better choose what these gain 
values need to be. 

An issue directly related to testing is that of post-processing. The post-processing code writ- 
ten to run on the previous prototype SANS should be updated/changed to enable its use on the 
newly designed SANS. Through post-processing test data, one can more easily run and re-run the 
tests in the goal of more easily optimizing the Kalman filter gains. The current version of the 
SANS software does not save the “raw” sensor data. This should be made possible. With the 
help of post-processing code, one needs only run a particular tilt/bench/cart test one time, and 
then take the raw data back, analyze it, adjust gains and re-ruh the test in order to optimize gains. 

There are hardware issues that remain and are areas for future work. The SANS uses a paddle- 
wheel type water speed sensor. This is a rather crude and inaccurate sensor, and should be 
upgraded to one which can deliver reliable data at the slower velocities under which the towfish 
and Phoenix AUV operate. Additionally, in order to conduct future at-sea testing, the DGPS 
antenna currently used on the SANS will have to be upgraded. In previous sea trials, the towfish 
would become fowled by seaweed. Under these conditions, the current DGPS antenna will not 
survive. Due to difficulties in acquiring software drivers for the Ethernet module, the network 
connection between the SANS and an external PC was not established during the course of this 



,S4 



thesis. In order to conduct at-sea testing, this network connection is a must, so work still remains 
in bringing this aspect of the SANS to fruition. 

Lastly, the final step in this research will be incorporation of the SANS into the NPS AUV. In 
this work, there should not be any significant changes to the hardware. However, there will be 
requirements in interfacing this hardware with the operating system running the NPS AUV. It 
may be beneficial from both a space-saving aspect as well as from the aspect of having more com- 
puting power, to transfer all software operation over to the Sun Voyager workstation which cur- 
rently accomplishes mission control for the NPS AUV. In this case, the issues of serial port 
communications, and A/D conversion wUl have to be readdressed before this can be successful. 
On the other hand, there is a distinct advantage to leaving the SANS as is and simply integrating 
it into the NPS AUV as another client in its established client/server environment. A detailed 
study of these alternatives will be required before the best solution can be developed. 
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APPENDIX A: Real Time Navigation Source Code(C++) 



A. TOWTYPES.H 

#ifndef TOETYPES_H 

#define TOETYPES_H 

#include <stdio.h> 

# include <dos.h> 

#include <time.h> 

#include "globals.h" y/ Types used by serial cominuni cat ions software 

#define GPSBLOCKSIZE 76// Size of Motorola @@Ea position message 
#define COMPSIZE 60 // Max size of compass message 

#define biasNumber 10 // Number of packets used to calculate initial bias 

#define ONE_G 32.2185// One g in feet per second 
#define GRAVITY 32.2185 // In feet per second 

#define TicksToSecs (x) ((double) ((10 * x) / 182)) 

typedef char ONEBYTE; 
typedef short TWOBYTE; 
typedef long FOURBYTE; 

typedef unsigned char UNSIGNED_ONEBYTE; 
typedef unsigned short UNS IGNED_TWOBYTE ; 
typedef unsigned long UNSIGNED_FOURBYTE ; 

// Holds lat/long expressed in milseconds 
struct latLongMilSec { 
long latitude; 
long longitude; 

); 



// Holds a latitude or longitude expressed in hours minutes and degrees 
struct T_GEODETIC { 

TWOBYTE degrees; 

UNS I GNE D_TWOB YTE minutes; 
double seconds; 

}; 

// Holds a latitude and longitude expressed as T_GEODETICs 
struct latLongPosit ion { 

T_GEODETIC latitude; 

T_GEODETIC longitude ; 



// Holds a grid position 
struct grid { 
double x,y,z; 

); 
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// 3 X 3 matrix 
struct matrix { 

float element [ 3 ] [ 3 ] ; 

); 

// 3 X 1 matrix or vector 
struct vector { 

float element [3 ]; 

}; 



// Oversize area to hold a GPS message 
typedef BYTE GPSdata[2 * GPSBLOCKSIZE] ; 



// Defines a type for holding compass messages 
typedef BYTE compData[2 * COMPSIZE] ; 



// Structure for 
// The positions 
/ / sample [ 0 ] : 
// sample [1] : 
// sample [2 ] : 
/ / sample [ 3 ] : 
/ / sample [ 4 ] : 
/ / sample [ 5 ] ; 
/ / sample [ 6 ] : 
/ / sample [7 ] : 



passing around various 

in the sample field of 

X acceleration 

y acceleration 

z acceleration 

phi 

theta 

psi 

water speed 
heading 



types of INS information, 
a stampedSample structure 



struct stampedSample { 

grid est; //position as estimated by the INS. 

float rawSample [ 8 ] ; //Original readings for post processing 
double sample[8]; //sampler converted sample, 
float deltaT; 
float current; 

}; 

#endif 



B. LOCATION.H 



//Conversion constants for location of 
//36:35:42.2N andl21 : 52 : 28 . 7W 

#define LatToFt 0.10134 //converts degrees Latitude to ft 
#define LongToFt 0.08156 //converts degrees Longitude to ft 
#define HemisphereConversion -1 //~1 if west of of Greenwich 

#define RADIANMAGVAR 0.261799 // Local area Magnetic variation in radians 



#define xyAccelLimit ONE_G// Max accell in x and y diretion 
#define zAccelLimit 2 * 0NE_G // Max accel in z direction 
#define rateLimit 0.872665// Max rotational rate in radians 
#define speedLimit 25.3 //Max water speed 
#define headingLimit 2 M_PI 



#define pScale 1.0 //roll 
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#define qScale 1.0 //pitch 
#define rScale 1.0 //yaw 

#define xAccelScale 1.34 //1. 078 //pitch 

#define yAccelScale 3.895 //roll 
#define zAccelScale 1.34 //1. 038 



#define pUnits (angular ) 
#define qUnits (angular ) 
#define rUnits (angular ) 



(pScale * ( ( (angular-2047 . 0 ) 

* 50.0) * (M_PI/180.0) ) 
(qScale * (( (angular-2047 . 0 ) 

* 50.0) * (M_PI/180.0) ) 

(rScale * (( (angular-2047 . 0 ) 

*50.0) * (M_PI/180.0) ) 



/ 2047.0 ) 
/ 2047.0 ) 
/ 2047.0 ) 



#define xAccelUnits ( linear) 
#define yAccelUnits ( 1 inear ) 
#define zAccelUnits ( linear ) 



(xAccelScale * ( ( linear-2047 . 0 ) / 
2047.0 ) * GRAVITY) 

(yAccelScale * (( linear-2047 . 0 ) / 
2047.0 ) * GRAVITY) 

(zAccelScale * (( linear-2047 . 0 ) / 



2047.0) * (2.0 * GRAVITY)) 

#define waterSpeedScale 1.0 //I. 827 

#define depthUnits (depth) (((depth - 819.0) / (4095.0-819.0)) 

* 180.0) 

#define waterSpeedUnits ( speed) (waterSpeedScale * ((speed - 

2047.0) / 2048.0) * 25.3) //feet per second 



#define radToDeg ( 18 0 . 0 /M_PI ) 
#define degToRad (M_PI/ 180.0) 
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C. TOWFISH.CPP 



# include 
#include 
#include 
#include 
#include 



<stdlib.h> 
<stdio ,h> 
<string .h> 
<iostream. h> 
<conio . h> 



#include " toetypes . h " 
# include "nav.h" 



int breakHandler ( void) ; 

void screenSetUp ( void) ; 

void printPos ition (const latLongPosit ion &) ; 

PROGRAM: Main 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Drives the navigator and its associated software. Counts 

the positions and displays each to the screen. Exited only when 
control break is entered at the keyboard. 

RETURNS : 0 

CALLED BY : none 

CALLS: initializeNavigator (nav.h) 

navPosit (nav.h) 
printPosition 
breakHandler 

★***★★★★*****★**★★★*★******************★***★★★**★*******★*★★**★★*** j 



int 

main (int argc, char *argv[]) 

{ 

ctrlbrk (breakHandler ) ; // trap all breaks to release com ports 

setcbrk(l) ; // turn break checking on at all times 

char *dataFile, *scriptFile, *attitudeFile ; 

switch (argc) { 
case 2 : 

scriptFile = new char [ strlen (argv [ 1 ] ) ] ; 

strcpy (scriptFile, argv [ 1 ]); //explicit script file only 
dataFile = "data" ; //default raw data file 
attitudeFile = "attitude"; 
break; 

case 3 : 

scriptFile = new char [strlen (argv[ 1] )] ; 

strcpy (scriptFile, argv [ 1 ]); //explicit script file 

dataFile = new char [ strlen (argv [ 2 ])] ; 

strcpy (dataFile, argv [ 2 ] ) ; //expl icit data file 

attitudeFile = "attitude"; 

break; 



60 



case 4 : 

scriptFile = new char [ strlen (argv [ 1 ] ) ] ; 

strcpy ( scriptFile, argv [ 1 ]);/ /explicit script file 

dataFile = new char [ strlen (argv [ 2 ])] ; 

strcpy (dataFile, argv[2 ]); //explicit data file 

attitudeFile = new char[strlen(argv[3])]; 

strcpy (att itudeFile , argv [ 3 ]);/ /explicit attitude file 

break; 

default : 

scriptFile = " script ";/ /default script file 
dataFile = " rawdata ";/ /default raw data file 
attitudeFile = "attitude"; 



clrscr ( ) ; 

cout « "\nWriting script information to " « scriptFile; 

cout « "\nWriting binary data to " « dataFile; 

cout « "\nWriting attitude data to " << attitudeFile << endl ; 

cout « " \nlnit ializing . . 

//Instantiate the navigator 

navigator navi ( scriptFile , dataFile, attitudeFile); 

latLongPosit ion currentLocation; // Lat/Long of most recent fix 
Boolean fixReceived = FALSE; //True if a new fix was recieved 
int fixCount=0; // Count of navigation fixes recieved 

//Initialize the navigator 

currentLocation = navi . init ializeNavigator ( ) ; 
gotoxy (1,6) ; 

cout « "Initialization Complete 1 \n" ; 
cout « "Initial Positionin''; 

//Print the initial position 

cout « "latitude: " « currentLocation . latitude . degrees « 

<< currentLocation . latitude .minutes « 

« currentLocation . latitude . seconds « endl; 
cout « "longitude: " « currentLocation . longitude . degrees « ' 

« currentLocation . longitude .minutes << 

« currentLocation . longitude . seconds ; 

screenSetUp ( ) ; 

while (TRUE) { 

// Attempt to get a fix from the navigator 
fixReceived = navi .navPosit (currentLocation) 
if (fixReceived) { 

// New fix recieved 
gotoxy (8,11) ; 
cout « ++fixCount; 
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} 



printPosition (currentLocation) ; 



/ 






PROGRAM : 

AUTHOR; 

DATE: 

FUNCTION: 

RETURNS : 

CALLED BY: 

CALLS: 



printPositon 

Eric Bachmann, Dave Gay 

11 July 1995 

Displays position to the screen 

void 

mail 

none 






void 

printPosition (const latLongPositionSc 

{ 

gotoxy ( 11 , 14 ) ; 

cout « posit . latitude .degrees << 
<< posit . latitude .minutes << 
gotoxy (12,15); 

cout « posit . longitude .degrees « 
« posit . longitude .minutes << 

) 



posit ) 



<< posit . latitude . seconds « endl; 



« posit . longitude . seconds « endl 






PROGRAM: 
AUTHOR : 
DATE : 
FUNCTION: 
RETURNS ; 
CALLED BY: 
CALLS : 



breakHandler 

Eric Bachmann, Dave Gay 

11 July 1995 

Cleans up com ports upon program exit. 
0 

main 

cleanup (port Bank. h) 






int 

breakHandler (void) 

{ 

COMports . cleanup ( ) ; 
exit ( 0 ) ; 

return 0; // keep the compiler happy 

) 
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y'*********************************************************************** 

PROGRAM : screenSetup 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 12 May 1996 

FUNCTION : Sets up the output screen 
RETURNS : 0 
CALLED BY: main 

CALLS : none 

★★****★**★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★*★★★★★★★★*★★*★★★★★**★★* j 

void 

screenSetUp (void) 

{ 

gotoxy (4, 11) ; 
cout « "Fix 

gotoxy (1, 14) ; 

cout « "Latitude: " « " \nLongitude : 

gotoxy (1, 17) ; 

cout « "Roll: " « "\nPitch: 

gotoxy (1,25) ; 
cout « "deltaT: 

int col ( 45 ) , row ( 1 ) ; 

gotoxy ( col , row++ ) ; 
cout « "x accel: "; 
gotoxy (col , row++ ) ; 
cout « "y accel : " ; 

gotoxy (col , row++ ) ; 
cout « "z accel: 
gotoxy (col , row++ ) ; 
cout « "phi dot : " ; 

gotoxy ( col , row++ ) ; 
cout « " theta dot : " ; 

gotoxy (col, row++ ) ; 
cout « "psi dot: 
gotoxy ( col , row++ ) ; 
cout « "water speed: 
gotoxy (col, row++ ) ; 
cout « "heading: " ; 

col = 45; 
row = 12 ; 

gotoxy (col, row++ ) ; 
cout « "x: "; 
gotoxy ( col , row++ ) ; 
cout << "y: 
gotoxy (col , row++ ) ; 
cout « " z : " ; 
gotoxy ( col , row++ ) ; 
cout « "phi: " ; 
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gotoxy { col , row++ ) ; 
cout « "theta: 
gotoxy ( col , row++ ) ; 
cout « "psi: 

gotoxy (45,20) ; 

cout << "Bias Values"; 

gotoxy (60,20) ; 

cout << "Current Values"; 



) 



D. NAV.H 



#ifndef 

#define 

#include 

#include 

#include 

#include 



_NAVIGATOR_H 
_NAVIGATOR_H 
<stdio .h> 

<f stream . h> 
<iostream. h> 
<math .h> 



#include 
#include 
# include 
#include 



" toetypes . h" 
" gps .h" 

" ins . h" 

" location . h" 



// Converts milseconds to degrees 

#define MSECS_TO_DEGREES (1.0/(1000.0 * 3600.0)) 

^ic***************-k***ic*ir******ie***********-k-kir-k*-k-k-k-k-k-k-k-k-k-kie'k***-k**-k****-kir 

CLASS: navigator 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Combines GPS and INS information to return the current 

estimated position. 



class navigator { 



public : 



//Constructor, opens script and data files 
navigator ( char scriptFile[] = "navScript " , char dataFile[] 
char attitudeFile [ ] = "navatt"): positionData (scriptFile) 
att itudeData (attitudeFile) , elapsedTime (0.0), f ixCount ( 0 ) , 
insSpeedSum (0.0) 

(if ( (rawData = f open (dataFi le , "wb")) == NULL) { 

cout « "NO RAW DATA RECORD";) 



= "navData", 

/ 

gpsSpeedSum (0.0) 



} 



//Destructor, closes script and data files 

-navigator ( ) (positionData . close ( ) ; attitudeData . close ( ) ; f close ( rawData) ; ) 
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//provides the navigator's best estimate of current position 
Boolean navPosit ( latLongPosit ionSc ) ; 

//Initialize the navigator 
latLongPosition ini t ializeNavigator ( ) ; 

private : 

float elapsedTime; // Tracks time since navigator was initialized 
int fixCount; // the number of position fixes obtained 

double gpsSpeed, insSpeed; 
double gpsSpeedSum, insSpeedSum; 

INS insl;//INS object instance. 

GPS gpsl;//GPS object instance. 

ofstream attitudeData; // Post processing attitude data, 
ofstream pos it ionData ; // Position script file 

FILE *rawData;// Post processing binary data file. 

latLongMilSec origin; //lat-long of navigational origin 

//Write position information to script file 
void writeScriptPosit ( int , latLongMilSecSc, char) ; 

//Write an INS packet and its timeStamp to the outPut file 
void writelnsData (const stampedSampleSc drPosition) ; 

//Write a GPS message to the outPut file, 
void writeGpsData (const GPSdata& sat Posit ion) ; 

//Returns the position in Miliseconds 
latLongMilSec getMilSec (const GPSdata&) ; 

//Convert position in milSec to degress, minutes, seconds and milsec 
latLongPosition milSecToLatLong ( const latLongMilSecSc) ; 

//Convert xy (grid) position to lat long 
latLongMilSec gridToMilSec ( const gridSc) ; 

//Converts lat /long to xy position 
grid milSecToGrid ( const latLongMilSecSc) ; 

//Parses and returns the time of a GPS message, 
double getGpsTime (const GPSdataSc rawMessage) ; 

//Parses and returns the velocity in fps of a GPS message, 
double getGpsVelocity (const GPSdataSc rawMessage); 

); 

#endif 
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E. NAV.CPP 



# include "nav.h" 

# include “signal .h" 

#define SIGFPE 8// Floating point exception 

^****ic***-*c*******ie**ie**ifieif*ic*********-k*icie-kic-k**-k*icieie-k-k***-k***-kif***-k***ic*-k 

PROGRAM: navPosit 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Provides the navigator's best estimate of current position. 
Attempts to obtain GPS and INS position fixes from the gps 
and ins objects and copies the most accurate fix available 
into the input argument ' navPosit ion ' . Writes the raw position 
fix data to the output file for post processing. Sets a return 
flag to indicate whether a valid fix was obtained. 

RETURNS: TRUE, a valid position fix is in the variable ' navPosit ion ' . 

FALSE, otherwise. 

CALLED BY: towf ish.cpp (main) 

CALLS: gpsPosition (gps.h) 

correct Posit ion (ins.h) 
insPosition (ins.h) 
getMilSec (nav.h) 
gridToMilSec (nav.h) 
milSecToGrid (nav.h) 
milSecToLatLong (nav.h) 
writeScript Posit (nav.h) 

void fpeNavPosit ( int sig) 

{if (sig == SIGFPE) cerr << "floating point error in navPosit\n" ; } 
Boolean 

navigator: : navPosit ( latLongPos it ionSc navPosition) 

{ 



signal (SIGFPE, fpeNavPosit); 

GPSdata sat Posit ion ; // the latest GPS position 

stampedSample drPosition; // the latest INS position 

latLongMilSec gpsMilSec; // the latest GPS position in milseconds 

latLongMilSec insMilSec; // the latest INS position in milseconds 

//Attempt to get the INS and GPS positions 
Boolean insFlag = insl . insPosition (drPosition) ; 

Boolean gpsFlag = gpsl . gpsPosit ion (satPosit ion ) ; 

//INS and GPS positions obtained? 
if (insFlag && gpsFlag) { 
gotoxy (20,11); 
cout « "GPS"; 

// Write INS packet and attitude info to an output file 
elapsedTime += drPosition.deltaT; 
writelnsData (drPosition) ; 

//Write GPS message to output file. 
writeGpsData (satPosition) ; 
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//Parse position from GPS messsage 
gpsMilSec = getMi iSec ( satPosition) ; 

//Write milsec position to script file 
writeScript Posit ( ++fixCount , gpsMilSec, *G'); 

//Pass GPS position to INS object for navigation corrections, 

insl . correctPosit ion (milSecToGrid (gpsMilSec) , getGpsTime ( satPos it ion 

//Covert position in mil sec to latitude and longitude. 

navPosition = mi iSecToLatLong (gpsMilSec) ; 

return TRUE; 

} 

else { 

//Only INS position obtained? 
if (insFlag) { 
gotoxy (20,11); 
cout « " " ; 

// Write INS Packet to output file. 
elapsedTime += drPosit ion . deltaT; 
writelnsData (drPosition) ; 

insMilSec = gridToMilSec (drPosition . est ) ; 

//Write milsec position to script file 
writeScriptPosit ( ++f ixCount , insMilSec, 'I'); 
navPosition = mi ISecToLatLong ( insMilSec ) ; 

insSpeed = drPosit ion . sample [ 6 ] ; 

return TRUE; 

) 

else { 

// Only GPS position obtained? 
if (gpsFlag) { 
gotoxy (20,11); 
cout « "GPS"; 

// Write GPS message to output file. 
writeGpsData (satPosition) ; 

//Parse position from GPS messsage 
gpsMilSec = getMilSec ( satPosition) ; 

//Write milsec position to script file 
writeScript Posit (++fixCount , gpsMilSec, ’G‘); 

//Pass GPS position to INS object for navigation corrections, 
insl . correct Position (milSecToGrid(gpsMilSec) , 

getGpsTime (satPosition) ) 
//Convert position in mil sec to lat/long. 
navPosition = milSecToLatLong (getMilSec ( satPosition) ) ; 

gpsSpeed = getGpsVelocity (satPosition) ; 

gpsSpeedSum += gpsSpeed; 

insSpeedSum += insSpeed; 

gotoxy (58,9) ; 

cout « gpsSpeed; 

gotoxy ( 58 , 10 ) ; 

cout « gpsSpeedSum / insSpeedSum; 
return TRUE; 

} 



67 



) 



else { 

return FALSE; // No new position available 

} 



) 



} 

^ic'kic-k-kic'k-k^ieicic'kiririeiricir'kicicieicieiciriricicic-kiriricic'kicirir-kieir'kicicir-kicicir’k'kiriric-kic-k-k-k-k-k-kiirif-k-k’k-k-k 

PROGRAM: wr i teScriptPosit 

AUTHOR: Eric Bachmann, Dave Gay 



DATE : 



11 July 1995 



FUNCTION: 

RETURNS : 
CALLED BY: 

CALLS: 



Writes the fix number, the position in milSec and the type 

of fix to the script file. 

void 

navPosit (nav.cpp) 
initialPosit (nav.cpp) 

None 

★ *****★■*■*★*★★****■*★***★★**★★*★***★*★*★*★★★★★★*★★★★*★*★★★★*★★★ 



/ 



void 

navigator : :wri teScriptPosit ( int fixNumber, latLongMi ISecSc posit, char fixType) 

{ 

positionData << fixNumber « ' ' 

<< posit . latitude << ' ' 

<< posit . longitude << ' ' 

<< fixType « ' ' 

<< elapsedTime « endl ; 

} 



^^★★★★★★★★★★★★★★★★★★★★★★*********** + ********************************-***** 

PROGRAM: writelnsData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Writes the packet and the time stamp contained in a stamped 

sample to the out put file for post processing. 

RETURNS: void 

CALLED BY: navPosit (nav.cpp) 

CALLS : None 

★ ★★*★★★***★*★•*★*★★*★★*★★★★****★*★★*★*★★★★★★★★***★**★•*•*★*•*■★★★★★★*■*■★★★★***★ ^ 



void 

navigator :: writelnsData (const stampedSample& drPosition) 

{ 



//MUST ADD CODE TO RECORD DATA FOR POST PROCESSING HERE 



//Output attitude data to a file 
attitudeData << 



elapsedTime « ' ' 

« drPosition . sample [ 0 ] « ' ' 

<< -1.0 * drPosition . sample [ 1 ] 



<< 


drPosition 


.sample [2] « ' ' 






<< 


(radToDeg 


* drPosition . sample 


[3] ) 


<< 


<< 


( radToDeg 


* drPosition . sample 


[4] ) 


<< 


<< 


( radToDeg 


* drPosition . sample 


[5] ) 


<< 



drPosition . sample [ 6 ] 
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« (radToDeg * drPosit ion . sample [ 7 ] ) « 
<< drPos it ion . current « endl; 



) 



PROGRAM : 
AUTHOR : 
DATE : 
FUNCTION: 

RETURNS : 
CALLED BY: 
CALLS : 



writeGpsData 

Eric Bachmann, Dave Gay 

11 July 1995 

Writes a raw GPS message to a binary output file for 

post processing. 

void 

navPosit (nav.cpp) 

None 






void 

navigator : :writeGpsData (const GPSdataSc satPosition) 

{ 

for( int j = 0; j < GPSBLOCKSIZE; j++) { 

putc (satPosition [ j ] , rawData) ; 

} 

) 



PROGRAM: init ializeNavigator 

AUTHOR: Eric Bachmann, Dave Gay 



DATE: 11 July 1995 

FUNCTION: Obtains an initial GPS fix for use as a navigational origin for 

grid positions used by the INS object. Saves the origin and passes 
it to the INS object in latLong form. 



RETURNS : TRUE 

CALLED BY: towfish (main) 

CALLS: gps Posit ion (gps.cpp) 

correctPosition (ins.cpp) 
getMilSec (nav.cpp) 
milSecToGrid (nav.cpp) 



★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ y 



latLongPosition 

navigator: : init ializeNavigator ( ) 

{ 

stampedSample biasPackets [biasNumber ] ; 

GPSdata satPosition; //gps position message 

// Loop until an initial GPS fix is obtained, 
while (! gpsl .gpsPosition (satPosition) ) {/**/) 

// Write GPS message for the grid origin to output file. 
writeGpsData (satPosition) ; 

//Save navigational origin for later grid position conversions, 
origin = getMilSec (satPosition) ; 

//Write the initial position to the script file 
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writeScriptPosit ( 0 , origin, *G'); 

//Pass time of first GPS fix to INS object initialization routine, 
insl . insSetUp (getGpsTime ( sat Position) , biasPackets) ; 

for (int i = 0; i < biasNumber; i++) { 

writelnsData (biasPackets [ i ] ) ; 

) 

insSpeed = biasPackets [biasNumber - 1 ]. sample [ 6 ] ; 

gpsSpeed = getGpsVelocity ( sat Posit ion) ; 

//Return the initial position to the caller, 
return milSecToLatLong (origin); 



PROGRAM igetMilSec 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Extracts a position in mili seconds from a Motorola (@@Ba) 

position contained in the input argument 'rawMessage* and returns it. 
RETURNS: The latitude and longitude in milseconds. 

CALLED BY : navPos i t ( nav . cpp ) 

init ializeNavigator (nav. cpp) 

CALLS : none . 



latLongMi ISec 

navigator : :getMilSec (const GPSdataSc rawMessage) { 



FOURBYTE temps4byte; 
latLongMilSec position; 



temps4byte 

temps4byte 

temps4byte 

temps4byte 

position . latitude 

temps4byte 
temps4byte 
temps4byte 
temps4byte 

position . !■ 



= rawMessage [ 15] ; 
= ( temps4byte«8 ) 
= ( temps4byte«8) 
= ( temps4byte«8) 

= temps4byte; 

= rawMessage [ 19 ] ; 
= ( temps4byte«8) 
= ( temps4byte«8) 
= ( temps4byte«8 ) 

e = temps4byte; 



+ rawMessage [ 16 ] ; 
+ rawMessage [ 17 ] ; 
+ rawMessage [18]; 



+ rawMessage [20 ] 
+ rawMessage [21 ] 
+ rawMessage [22 ] 



return position; 

} 
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PROGRAM : milSecToLatLong 
AUTHOR; Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Converts a position expressed in totally in mili seconds to 
degress, minutes, seconds and mili seconds and returns the result. 
RETURNS: The position in degress, minutes, seconds and mili seconds. 

CALLED BY: navPosit (nav.cpp) 

CALLS : none 

★★★★****★★★**★******★*★**★★*★***★*★★*★★***★**★★*★**★****★*****★*★★★★★*★*★ j 



latLongPosit ion 

navigator: :milSecToLatLong (const latLongMilSec& milSec) { 

latLongPosit ion position; 
double degrees, minutes; 

degrees = ( double ) mil Sec . latitude * MSECS_TO_DEGREES; 
posit ion . latitude . degrees = (TWOBYTE) degrees ; 

if (degrees < 0) 

degrees = fabs (degrees) ; 

minutes = (degrees - (TWOBYTE) degrees ) * 60.0; 

posit ion . latitude .minutes = (TWOBYTE)minutes; 

position . latitude . seconds = (minutes - (TWOBYTE) minutes ) * 60.0; 

degrees = (double ) mi ISec . longitude * MSECS_TO_DEGREES; 
position . longitude .degrees = (TWOBYTE) degrees ; 

if (degrees < 0) 

degrees = fabs (degrees) ; 

minutes = (degrees - (TWOBYTE) degrees) * 60.0; 
position . longitude .minutes = (TWOBYTE) minutes ; 

posit ion . longitude . seconds = (minutes - (TWOBYTE) minutes ) * 60.0; 
return position; 



} 
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PROGRAM : gr idToMi iSec 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION .-Convert a grid position to a latitude and longitude in mil- 
seconds and returns the result. 

RETURNS:The latitude and longitude in milseconds. 

CALLED BY:navPosit (nav.cpp) 

CALLS : none 

void fpeGridToMilSec ( int sig) 

{if (sig == SIGFPE) cerr « "floating point error in gridToMilSecXn" ; ) 
latLongMilSec 

navigator : igridToMilSec (const grid& posit) 

{ 

signal (SIGFPE, fpeGridToMilSec) ; 
latLongMilSec latLong; 

//converts grid in ft to latitude 

latLong . latitude = origin . latitude + (posit. x / LatToFt) ; 

//converts grid in ft to longitude 
latLong . longitude = origin . longitude + 

HemisphereConversion * (posit. y / LongToFt) ; 
return latLong; 

} 

PROGRAM rmilSecToGr id 

AUTHOR -.Erie Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION : Convert a latitude and longitude expressed in milseconds to 
a grid position based on the lat/long of the grid origin. 

RETURNS: The grid position 
CALLED BY:navPosit (nav.cpp) 

initializeNavigator (nav.cpp) 

CALLS : none 

COMMENTS : altitude is always assumed to be zero. 






//Converts latitude/ longitude to xy coords in ft from origin 
grid 

navigator : :milSecToGrid (const latLongMilSec^ posit) 

{ 

grid position; 

position. X = (posit . latitude - origin. latitude) * LatToFt; 
posit ion. y = HemisphereConversion * 

(posit . longitude - origin . longitude ) * LongToFt; 

position. z = 0; 

return position; 

} 
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PROGRAM : getGpsTime 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Parse the time of a gps message. 

RETURNS: The time of the gps message in seconds 
CALLED BY:navPosit (nav.cpp) 

initializeNavigator (nav.cpp) 

CALLS : none 
double 

navigator :: getGpsTime ( const GPSdataSc rawMessage) 

{ 

UNSIGNED_ONEBYTE tempchar, hours, minutes; 

UNSIGNED_FOURBYTE tempu4by te ; 

double seconds ; 



hours = rawMessage [ 8] ; 
minutes = rawMessage [ 9 ] ; 



tempchar 

tempu4byte 

tempu4byte 

tempu4byte 

tempu4byte 

seconds = 



= rawMessage [ 10 ] ; 

= rawMessage [ 11 ] ; 

= ( tempu4byte<<8 ) + rawMessage [ 12 ] ; 

= ( tempu4byte<<8 ) + rawMessage [ 13 ] ; 

= ( tempu4byte<<8 ) + rawMessage [ 14 ] ; 
(double) tempchar + ( ( (double) tempu4byte) /I . OE+9 ) ; 



return hours * 3600.0 + minutes * 60,0 + seconds; 

} 



PROGRAM : getGpsVelocity 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION : Parse the velocity out of a gps message. 

RETURNS: The velocitiy of the gps message in feet per second 
CALLED BY:navPosit (nav.cpp) 

initializeNavigator (nav.cpp) 



CALLS : none 



double 

navigator :: getGpsVelocity (const GPSdataSc rawMessage) 

{ 

UNSIGNED_ONEBYTE tempchar=rawMessage [ 3 1 ] ; 

return (double )( 3 . 2804 * ((tempchar << 8) + rawMessage [ 32 ] ) / 100.00); 

} 
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F, GPS.H 



#ifndef _GPS_H 
#define _GPS_H 

#include <iostream.h> 
#include <fstream.h> 
#include <conio.h> 



# include "portbank.h" 
#include '' toetypes . h" 
#include "gpsbuf f .h" 



CLASS :gps 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

MODIFIED: 15 May 199 6 by Eric Bachmann Sc Randy Walker 
FUNCTION : Reads GPS messages from the GPS buffer. Checks for valid 
checksum and minimun number of satelites in view. 



class GPS { 
public : 

//Class Constructor 

GPS() : portl (COMports . Init (COMl, BYTE ( 4 ) , b9600, 

NOPARITY, BYTE(8), BYTE ( 1 ) , XON_XOFF, messages)) { ) 

//returns the latest gps position and a flag 
Boolean gps Posit ion (GPSdatafi:) ; 

private : 

//buffer for gps data 
GPSbuffer messages; 

//instantiates serial port communications on comm2 
buf f eredSerialPortSc portl; 

//calculates the check sum of the message 
Boolean checkSumCheck (const GPSdata) ; 



) ; 

#endif 
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G. GPS.CPP 



# include <math.h> 

#include "gps.h" 

NAME : gps Posit ion 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

MODIFIED: 15 May 1995 BY Eric Bachmann & Randy Walker 

FUNCTION: Determines if an updated gps position message is available and 
copies it into the input argument ' rawMessage ' . If the message 
has a valid checksum, was obtained with at least three 

satelites in view, and contained the differential correction, a 'TRUE' 
is returned to the caller, indicating that the message is valid. 

RETURNS: TRUE, if a valid position message is contained in the 
input argument . 

CALLED BY: navPosit (navigator . h) 

CALLS: Get (buffer.h) 

checkSumCheck (gps.h) 



Boolean 

GPS : : gpsPosit ion (GPSdataSc rawMessage) 

{ 

Boolean checkSumFlag; 
unsigned long Mask(4); 

if (messages .Get (rawMessage) ) { 

// Check for a valid check sum and more the 3 satelites and DGPS 
return Boolean ( (checkSumCheck (rawMessage) ) ScSc ( rawMessage [ 39 ] > 3) 
ScSc ( (rawMessage [GPSBLOCKSIZE - 4] & Mask) == Mask) ) ; 



} 

else { 

return FALSE; //No updated position is available. 

} 



} 



75 



PROGRAM : checkSumCheck 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

MODIFIED: 15 May, 199 6 by Eric Bachmann Sc Randy Walker 

FUNCTION : Takes an exclusive or of bytes 2 through 78 in a Motorola format 
(@@EA) position message and compares it to the checksum of the 
message . 

RETURNS: TRUE, if the message contains a valid checksum 
CALLED BY: gpsPosition (gps) 

CALLS : none 



Boolean 

GPS :: checkSumCheck (const GPSdata newMessage) 

{ 

BYTE chkSum ( 0 ) ; 

for (int i = 2; i < GPSBLOCKSIZE - 3; i++) { 

chkSum newMessage [ i ] ; 

} 

return Boolean (chkSum == newMessage [GPSBLOCKSIZE - 3]); 



H. INS.H 



#ifndef _INS_H 
#define _INS_H 



#include 
#include 
#include 
#include 
# include 
#include 
#include 



<time .h> 
<math . h> 

<dos . h> 
<stdio .h> 
<conio . h> 

<f stream. h> 
<iostream.h> 



#include 

#include 

#include 



" toe types .h" 
” location .h" 
" sampler . h" 



CLASS : ins 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Takes in linear accelerations, angular rates, speed and 

heading information and uses kalman filtering techniques to return 
a dead reconing position. 



class INS { 
public : 
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//Constructor initializes gains 
INS ( ) ; 



-INSO {} 



//returns the ins estimated position 
Boolean ins Posit ion (stampedSampleS: ) ; 



//Updates the x, y and z of the vehicle posture 
void correctPosition (const grid&, double); 



//Sets posture to the origin and developes initial biases 
void insSetUp (double, stampedSample* ) ; 

private : 



double posture [6]; 
double velocit ies [ 6 ] ; 

double current [3]; 



// ins estimated posture (x y z phi theta psi) 
// ins estimated linear and angular velocities 
// x-dot y-dot z-dot phi -dot theta-dot psi -dot 
// ins estimated error current 
// (x-dot y-dot z-dot) 



double lastGPStime; //time of last gps position fix 

sampler saml ; //sampler instance 



matrix rotat ionMatrix; //body to euler transformation matrix 

double biasCorrection[8] ; //Software bias corrections for IMU rate 

//sensors 



// Kalman filter gains. 

float Konel, Kone2, Ktwo, Kthreel, Kthree2, Kfourl, Kf our2 ; 

// Finds the difference between two times of struct time type 
double findDeltaT (struct timeSc next, struct timeS: last) ; 

// Transforms from body coordinates to earth coordinates 
// and removes the gravity component 
void transformAccels (double[]); 

// Transforms water speed reading to x and y components 
void transformWaterSpeed (double, double[]); 

// Tranforms body euler rates to earth euler rates, 
void transf ormBodyRates (double [] ) ; 

// Euler integrates the accelerations and updates the velocities 
void updateVelocit ies ( stampedS ample & ) ; 

// Euler integrates the velocities and update the posture 
void updatePosture (stampedSampleSc ) ; 
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// Builds the body to euler rate matrix 
matrix buildBodyRateMatrix ( ) ; 

// Builds the body to earth rotation matrix 
void buildRotationMatrix ( ) ; 

//Calculates the imu bias correction during set up 
void calculateBiasCorrections (stampedSample* ) ; 

//Applies bias corrections to a sample 
void applyBiasCorrect ions (double sample []); 






// Post multiply a matrix times a vector and return result, 
vector operator* (matrix&, double []) ; 

#endif 



I. INS.CPP 

#include <iostream.h> 

#include "signal. h” 

#include "ins.h" 

#define SIGFPE 8// Floating point exception 

PROGRAM: ins (constructor) 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION .’Constructor initializes kalman filter gains and linear and 
angular velocities. 

RETURNS: nothing 

CALLED BY: navigator class 

CALLS : none 

★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★ j 



INS::INS() : Konel(O.l), Kone2(0.5), 
Ktwo (0.6), 

Kthreel ( 0 . 5 ) , Kthree2 ( 0 . 5 ) , 
KfourKO.5), Kfour2(0.5) 



velocities [ 0 ] = 
velocities [ 1 ] = 
velocities [ 2 ] = 
velocities [ 3 ] = 
velocities [ 4 ] = 
velocities [ 5 ] = 



0.0;// X dot 
0.0;// y dot 
0.0;// z dot 
0.0;// phi dot 
0.0;// theta dot 
0.0;// psi dot 



//Set posture to straight and level at the origin, 
posture [0] = 0.0; 
posture [1] = 0.0; 
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posture [2] = 0.0; 
posture [3] = 0.0; 
posture [4] = 0.0; 
posture [5] = 0.0; 



// Initialize error current to zero 
current [0] = 0.0; 

current [1] = 0.0; 

current [2] = 0.0; 



PROGRAM: findDeltaT 

AUTHOR: Eric Bachmann, Dave Gay , 

DATE: 11 July 1995 

FUNCTION .-Converts two times stored in the time structure type of 
dos.h into the time in seconds and returns the difference. 

RETURNS : difference in seconds between the two input times. 

CALLED BY; ins Posit (ins.cpp) 

CALLS: none 

icicicic-kicicihic-k*ic*i(ic-ic-k-ic*-icic-ic-ic'kicic-icicic'kic'k'k’k-k-k*’ic-k-k-k'k'k'k'k-kir'k-ic'k-k-ic'kic'k'k'k-k-k-k-k-k'k'k-k'k'k'k'k-k'k-k'k j 

double 

INS :: findDeltaT (struct timeSc next, struct timeSc last) 

{ 

double present, past; 

present = next.ti_hour * 3600.0 + next.ti_min * 60.0 
+ next.ti_sec + next.ti_hund / 100.0; 
past = last.ti_hour * 3600.0 + last.ti_min * 60.0 
+ last.ti_sec + last.ti_hund / 100.0; 

// Did 2400 occur between present and past? 
if (present < past) { 
present += 86400.0; 

) 

return present - past; 

} 
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PROGRAM: insPosit 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Make dead reckoning position estimation using kalman 

filtering. Inputs are linear accelerations, angular rates, speed and 
heading. Primary input data is obtained from a sampler object via the 
getSample method. This data is stored in the sample filed of a 
stampedSample structure called newSample. The sample field is then 
used as a working variable as the linear accelerations and angular 
rates it contains are converted to earth coordinates and integrated 
to determine current velocities and posture. The data is complimentary 
filtered against itself, speed and magnetic heading. 

RETURNS : pos it ion in grid coordinates as estimated by the INS 
CALLED BY: navPosit (nav.cpp) 

CALLS: getSample ( sampler . cpp ) 

findDeltaT (ins. cpp) 
transf ormBodyRates (ins. cpp) 
bui IdRotationMatrix (ins. cpp) 
trans f ormAccels (ins) 
transf ormWaterSpeed (ins) 

void fpelnsPosit ( int sig) 

{if (sig == SIGFPE) cerr << "floating point error in insPosit\n" ; } 

Boolean 

INS: : insPosit ion ( stampedSampleSc newSample) 

{ 

signal (SIGFPE, fpelnsPosit) ; 

double thetaA, phiA, xincline, yincline; // Working variables 
double waterSpeedCorrection [ 3 ] ; // Filter correction for drift 

// and water speed 

if (saml .getSample (newSample) ) { 

applyBiasCorrect ions (newSample . sample) ; 

//Set output precision and fixed format 
cout .precision ( 6) ; 
cout . set f ( ios : : f ixed) ; 

//Display linear accelrations and angular rates 
for (int j = 0; j < 8; j++) { 

gotoxy ( 59 , j + 1 ) ; 
cout « newSample . sample [ j ] ; 

} 

//Display time delta to the screen. 

gotoxy (9,25) ; 

cout << newSample . deltaT; 

xincline = newSample . sample [ 0 ] / GRAVITY; 

yincline = newSample . sample [ 1 ] / (GRAVITY * cos (posture [4])); 
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if ( fabs (yincline) > 1.0) { 

static int inclineCount ( 0 ) ; 
gotoxy (1,24) ; 

cout « "Inclination errors: 0" << ++ incl ineCount « endl ; 
return FALSE; 

} 

//Calculate low freq pitch and roll 
thetaA = as in (xincline ) ; 
phiA = -asin (yincline) ; 

//Transform body rates to euler rates. 
transformBody Rates (newSample . sample) ; 

//Calculate estimated roll rate (phi-dot) . 

velocities [ 3 ] = newSample . sample [ 3 ] + Konel * (phiA - posture[3]); 

//Calculate estimated pitch rate (theta-dot) . 

velocities [ 4] = newSample . sample [ 4 ] + Kone2 * (thetaA - posture[4]) 
//Calculate estimated heading rate (psi-dot) . 

velocities [ 5 ] = newSample . sample [ 5 ] + Ktwo * (newSample . sample [7 ] - 
posture [ 5 ] ) ; 

//integrate estimated pitch rate to obtain pitch angle 
posture [3] += newSample . deltaT * velocities [ 3 ] ; 

//integrate estimated roll rate to obtain roll angle 
posture[4] += newSample . deltaT * velocit ies [ 4 ] ; 

//integrate estimated yaw rate to obtain heading 
posture[5] += newSample . deltaT * velocit ies [ 5 ] ; 

//Display roll and pitch 
gotoxy (8,17) ; 

cout « (posture[3] * radToDeg) ; 
gotoxy (8,18) ; 

cout « (posture [4] * radToDeg); 

buildRotat ionMatrix ( ) ; 

//Transform accels to earth coordinates 
transformAccels (newSample . sample ) ; 

//Transform water speed to earth coordinates 

transformWaterSpeed (newSample . sample [ 6 ] , waterSpeedCorrect ion) ; 

// Subtract out previous velocity and apply statistical gain 
waterSpeedCorrect ion [ 0 ] = Kthreel * (waterSpeedCorrect ion [ 0 ] - 

velocities [0]); 

waterSpeedCorrect ion [ 1 ] = Kthree2 * (waterSpeedCorrect ion [ 1 ] - 

velocities [ 1 ] ) ; 

// Determine filtered accelerations 

newSample . sample [ 0 ] += waterSpeedCorrection[0]; 

newSample . sample [ 1] += waterSpeedCorrect ion [ 1 ] ; 
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//Integrate accelerations to obtain velocities 
velocities [ 0 ] += newSample . sample [ 0 ] * newSample.deltaT; 

velocities [ 1] += newSample . sample [ 1 ] * newSample.deltaT; 
velocities [ 2 ] += newSample . sample [ 2 ] * newSample.deltaT; 

//Integrate velocities to obtain posture 

posture[0] += (velocities [ 0 ] + current[0]) * newSample.deltaT; 
posture [1] += (velocities [ 1 ] + current [1]) * newSample.deltaT; 
posture [2] += velocities [ 2 ] * newSample.deltaT; 



newSample . current = sqrt (current [ 0 ] * current [0] + 

current [1] * current [1]); 



newSample . sample [ 0 ] 
newSample . sample [ 1 ] 
newSample . sample [ 2 ] 
newSample . sample [ 3 ] 
newSample . sample [ 4 ] 
newSample . sample [ 5 ] 



posture [ 0 ] ; 
posture [ 1] ; 
posture [2 ] ; 
posture [ 3 ] ; 
posture [4] ; 
posture [ 5 ] ; 



//Display current location and posture 
for (j = 0; j < 6; j++) ( 

gotoxy (52, j+12 ) ; 
cout « posture[j]; 

} 

newSample . est .X = posture[0]; 
newSample . est .y = posture[l]; 
newSample . est . z = posture [2]; 



return TRUE; 

) 

else { 

return FALSE;// New IMU information was unavailable. 

} 

} 

PROGRAM: correct Posit ion 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Reinitializes the INS based on a known position and compute 
apparent current based on past accumulated errors of the INS. It is 
called by the navigator each time a new GPS (true) fix is obtained. 
RETURNS: void 

CALLED BY: navPosit (nav) 

CALLS : none 



void 

INS :: correctPosit ion ( const gridSc truePosit, double positTime) 

{ 

double deltaT; 

// Correct for new day if necessary 
if (positTime < lastGPStime) ( 
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positTime += 86400; 

) 

// Find time since last gps fix. 
deltaT = positTime - lastGPStime; 

// Detemine INS error since last gps fix 
double deltaX = truePosit.x - posture [0]; 
double deltaY = truePosit.y - posture[l]; 

// Reinitialize posture to known position (gps fix) 
posture [0] = truePosit.x; 

posture [1] = truePosit.y; 

posture [2] = 0.0; //Unit is assumed to be on the surface 

// Add gain filtered error to previous errors 
current [0] += Kfourl * (deltaX / deltaT); 
current[l] += Kfour2 * (deltaY / deltaT); 

// Display new error current values 
for(int j = 0; j < 3; j++) { 

gotoxy ( 60 , j+21 ) ; 
cout « current [j]; 

) 

// Display updated posture 
for (j = 0; j < 6; j++) { 

gotoxy ( 52 , j +12 ) ; 
cout « posture[j]; 

} 

// Save the time of the gps fix for next calculation 
lastGPStime = positTime; 
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PROGRAM: insSetUp 

AUTHOR: Eric Bachmann, Dave Gay 



DATE: 11 July 1995 

FUNCTION: Initializes the INS system. Sets 
Initializes the heading using magnetic 
the times of the last GPS fix and last 
RETURNS: void 

CALLED BY: init ial izeNavigator (nav) 

CALLS : calulateBiasCorrections ( ins) 
getSample (sampler) 
buildRotat ionMatrix (ins) 
transformWater Speed (ins) 



the posture to the origin, 
compass information. Initilizes 
IMU information. 



void fpelnsSetUp ( int sig) 

{if (sig == SIGFPE) cerr « "floating point error in inSetUp\n” ; } 
void 

INS ;: insSetUp (double originTime, stampedSample* biasPackets) 

( 

signal (SIGFPE, fpelnsSetUp) ; 

//Initialize the sampler 
saml . initSampler ( ) ; 

//set imu biases 

calculateBiasCorrect ions (biasPackets) ; 

//set initial true heading 

posture[5] = biasPackets [biasNumber-1 ]. sample [7 ] ; 

//set initial speed 
buildRotat ionMatrix ( ) ; 

transformWaterSpeed (biasPackets [biasNumber-1] .sample [6] , velocities) ; 

// initialize times 
lastGPStime = originTime; 

//Display initial error current values 
for (int j = 0; j < 3; j++) { 

gotoxy ( 60 , j +21 ) ; 
cout « current [j]; 

) 

} 
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y*********************************************************************** 
PROGRAM: transf ormAccels 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Trans forms linear accelerations from body coordinates to 

earth coordinates and removes the gravity component in the z direction. 
RETURNS: void 
CALLED BY: navPosit 

CALLS : none 

★ *★★**★★*★*★**★★*★*★★★***★*★★**★★*****★*★******★****★*★★★★★★*★★**★★★****•*■ y 



void 

INS :: transf ormAccels (double newSample[]) 

{ 

vector earthAccels; 



newSample [ 0 ] 
newSample [ 1 ] 
newSample [2 ] 



-= GRAVITY * 
+= GRAVITY * 
+= GRAVITY * 



sin (posture [4] ) 
sin (posture [ 3 ] ) 
cos (posture [3] ) 



* cos (posture [ 4 ]) ; 

* cos (posture [ 4 ]) ; 



earthAccels 



rotat ionMatrix * newSample; 



newSample [ 0 ] 
newSample [ 1 ] 
newSample [ 2 ] 



earthAccels . element [0 ] ; 
earthAccels .element [1 ] ; 
earthAccels .element [2 ] ; 



) 



y*********************************************************************** 

PROGRAM : t rans f ormWat erSpeed 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Trans forms water speed into a vector in earth coordinates 
and returns them in the speedCorrection variable. 

RETURNS: void 

CALLED BY: navPosit 

CALLS: none 

***************ifif*****i(*iir************-*r-k-k*-k-k-k**-*r****-*r-*r*-k*******’k****'k***'kic^ 



void 

INS :: transf ormWaterSpeed (double waterSpeed, double speedCorrection [] ) 

( 

double water [3] = {waterSpeed, 0.0, 0.0); 
vector waterVelocit ies = rotat ionMatrix * water; 



speedCorrection [0] 
speedCorrection [ 1 ] 
speedCorrection [2] 



waterVelocities . element [ 0 ] ; 
waterVelocit ies . element [ 1 ] ; 
waterVelocities . element [ 2 ] ; 
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PROGRAM: trans formBody Rates 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Tranforms body euler rates to earth euler rates 

RETURNS : none 

CALLED BY: insPosit 

CALLS: buildBodyRateMatrix 

★ ★★★★★★★★★★★★★★■A-*** j 



void 

INS :: trans formBodyRates (double newSample [ ] ) 

{ 

vector earthRates = buildBodyRateMatrix ( ) * & (newSample [ 3 ]) ; 

newSample [3] = earthRates . element [ 0 ] ; 

newSample [4] = earthRates . element [ 1 ] ; 

newSample[5] = earthRates . element [ 2 ] ; 

) 



PROGRAM: buildBodyRateMatrix 
AUTHOR: Eric Bachmann, Dave Gay 



DATE: 11 July 1995 

FUNCTION: Builds body to Euler rate translation matrix. 

RETURNS: rate translation matrix 

CALLED BY: insPosit 
CALLS : none 






matrix 

INS: : buildBodyRateMatrix ( ) 

{ 

matrix rateTrans ; 



float tth 
sphi 
cphi 
cth 



tan (posture [4] ) , 
sin (posture [3 ] ) , 
cos (posture [ 3 ] ) , 
cos (posture [ 4 ] ) ; 



rateTrans 

rateTrans 

rateTrans 

rateTrans 

rateTrans 

rateTrans 

rateTrans 

rateTrans 

rateTrans 



. element [ 0 ] 
. element [ 0 ] 
. element [ 0 ] 
. element [ 1 ] 
.element [ 1 ] 
.element [ 1] 
.element [2 ] 
-element [2 ] 
. element [ 2 ] 



[0] 


= 


1.0; 


[1] 


= 


tth * 


[2] 


= 


tth * 


[0] 


= 


0.0; 


[1] 


= 


cphi ; 


[2] 


= 


-sphi 


[0] 




0.0; 


[1] 


= 


sphi 


[2] 


= 


cphi 



sphi ; 
cphi ; 



/ cth; 
/ cth; 



return rateTrans; 

) 
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PROGRAM: buildRotat ionMatr ix 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION .-Sets the body to earth coordinate rotation matrix. 
RETURNS: void 



CALLED BY: ins Posit 

insSetUp 

CALLS : none 



void 

INS: :buildRotationMatrix ( ) 

{ 

float spsi = sin (posture [ 5 ]) , 
cpsi = cos (posture [ 5 ]) , 
sth = sin (posture [ 4 ]) , 
sphi = sin (posture [ 3 ]) , 
cphi = cos (posture [3 ]) , 
cth = cos(posture[4]); 

rotationMatrix . element [ 0 ][ 0 ] = 
rotationMatrix. element [ 0 ][ 1] = 

rotationMatrix. element [ 0 ] [2 ] = 
rotationMatrix. element [1] [0] = 
rotationMatrix . element [ 1 ][ 1 ] = 

rotationMatrix. element [ 1] [2] = 
rotationMatrix. element [2 ][ 0 ] = 
rotationMatrix. element [ 2 ][ 1] = 
rotationMatrix. element [2] [2] = 

) 



cpsi * 


cth; 








( cpsi 


* sth * 


sphi) “ 


(spsi * 


cphi ) 


(cpsi 


* sth * 


cphi) + 


(spsi * 


sphi ) 


spsi * 


cth; 








(cpsi 


* cphi) 


+ (spsi 


* sth * 


sphi) 


( spsi 


* sth * 


cphi) - 


(cpsi * 


sphi ) 



“Sth; 

cth * sphi ; 
cth * cphi ; 



PROGRAM:post multiplication operator * 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION : Post multiply a 3 X 3 matrix times a 3 X 1 vector and 
return the result. 

RETURNS: 3X1 vector 

CALLED BY: 



CALLS: None 



vector 

operator* (matrixSc transform, double state[]) 

{ 

vector result; 

for (int i = 0; i < 3; i++) { 

result . element [ i ] = 0.0; 
for (int j = 0; j < 3; j++) { 
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) 



result . element [ i ] += transform. element [ i ][ j ] 



state[ j] ; 



★ 



) 

return result; 

} 

PROGRAM : calculateBiasCorrect ions 
AUTHOR .‘Erie Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Calculates the initial imu bias by averaging a number of 

imu readings. 

RETURNS : none 

CALLED BY : insSetup 

CALLS : none 

void fpeCalculateBiasCorrections ( int sig) 

{if (sig == SIGFPE) cerr << “floating point error in 
CalculateBiasCorrect ions\n “ ; } 

void 

INS: : calculateBiasCorrect ions ( stampedSample* biasSample) 

{ 

signal (SIGFPE, fpeCalculateBiasCorrections) ; 

biasCorrection [3 ] = 0.0; 

biasCorrect ion [ 4 ] = 0.0; 

biasCorrection [ 5 ] = 0.0; 

for (int i = 0; i < biasNumber; i++) { 

while ( 1 saml . get Sample (biasSample [ i ]) ) {/* */}; 

biasCorrect ion [ 3 ] += biasSample[i].sample[3]; 
biasCorrect ion [ 4 ] += biasSample[i].sample[4]; 
biasCorrection [ 5 ] += biasSample [ i ]. sample [5 ] ; 

} 

// Find the average of the biasNumber packets 
biasCorrection [3 ] = -(biasCorrection[3]/biasNumber); 
biasCorrect ion [ 4 ] = -(biasCorrection[4]/biasNumber); 

biasCorrect ion [ 5 ] = - (biasCorrect ion [ 5 ] /biasNumber ) ; 

//Output the initial bias values 
for (int j=3;j<6; j++) { 

gotoxy (45, j+18) ; 
cout « biasCorrect ion [ j ] ; 

} 

} 
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PROGRAM: applyBiasCorrect ions 
AUTHOR: Eric Bachmann, Dave Gay 



DATE: 11 July 1995 
FUNCTION: Applies 

RETURNS: void 

CALLED BY: insPosi 

CALLS : none 



updated bias corrections to a sample. 



t 









/ 



void 

INS: : applyBiasCorrect ions (double sample [] ) 

{ 

static const float biasWght ( 0 . 9999 ) , sampleWght (0.0001); 



//Calculate updated 
biasCorrect ion [3 ] = 

biasCorrection [ 4 ] = 

biasCorrect ion [ 5 ] = 



bias values 

(biasWght * biasCorrection [ 3 ] ) 

- (sampleWght * sample [3]); 
(biasWght * biasCorrect ion [ 4 ] ) 

- (sampleWght * sample[4]); 
(biasWght * biasCorrection [ 5 ] ) 

- (sampleWght * sample[5]); 



//Apply the 
sample [3] + 
sample [4] + 
sample [5] + 



bias to the sample 
= biasCorrection [ 3 ] ; 
= biasCorrection [ 4 ] ; 
= biasCorrection [5]; 



// Output the biases 
for(int j = 3; j < 6; j++) { 

gotoxy (45, j+18) ; 
cout « biasCorrect ion [ j ] ; 



} 
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J. SAMPLER .H 



#ifndef 


_SAMPLER_H 


#define 


_SAMPLER_H 


#include 


<time .h> 


#include 


<math . h> 


iinclude 


<dos . h> 


#include 


<conio.h> 


#include 


<stdio .h> 


# include 


"toe types .h 


# include 


" location . h 


#include 


"a2d.h" 


#include 


" compass .h" 



#define MAX_SAMPLE_NUM 1000 
const int INBUFFSIZE = 512; 



CLASS : sampler 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 15 May 1996 

FUNCTION : Formats , timestamps, low pass filters and limit checks IMU, 
water-speed and heading information. 

COMMENTS: This class is extremely dependent upon the specific 

hardware configuration. It is designed to isolate to the INS from 
these particulars. 



class sampler { 
public : 

//Class Constructor 

sampler(): sampl e Index ( 0 ) , subSamp le Index ( 0 ) , 
samplePeriod (a2dl . chcnt * a2dl.delta_t * 0.000001) {} 

//Initializes Sampler 
Boolean initSampler ( ) ; 

//checks for the arrival of a new sample and formats it. 
Boolean getSample ( stampedSample&) ; 

private : 

compass compl ; //Compass instance 
a2dClass a2dl; //A2D instance 
float sample [MAX_SAMPLE_NUM] [8] ; 
int subSample Index; 
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int samp le Index; 



int sampleCount; 
float samplePeriod; 

Boolean readSamples ( stampedSample& newSample) ; 

void f ilterSample (stampedSampleSc newSample); 

void formatSample (stampedSampleSc newSample); 

void increment ( int& index) 

{ if (+ + index == MAX_SAMPLE_NUM ) index = 0;} 

void decrement ( int& index) 

{ if (--index < 0) index = MAX_SAMPLE_NUM - 1;} 

}; 

#endif 
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K. SAMPLER.CPP 



#include <iostream . h> 

#include <conio.h> 

# include " sampler. h" 

PROGRAM: initSampler 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 12 May 1995 

MODIFIED: 15 May 1996 by Eric Bachmann & Randy Walker 
FUNCTION: Initializes the compass and the A2D module. 
RETURNS : TRUE 
CALLED BY: 

CALLS: initCompass ( ) , A2D member functions 

Boolean 

sampler : : initSampler ( ) 

{ 

compl . initCompass ( ) ; 

a2dl . setRmsOf f ( ) ; 
a2dl . setSequencer ( ) ; 
a2dl . lockTrigger ( ) ; 
a2dl . resetFifo ( ) ; 
a2dl . setFifo ( ) ; 
a2dl .unlockTrigger ( ) ; 
a2dl . setTrigger ( ) ; 

return TRUE; 

} 
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PROGRAM: get Sample 

AUTHOR: Eric Bachmann, Randy Walker 
DATE: 15 May 1996 

FUNCTION : Prepares raw sample data for use by the INS 
RETURNS : TRUE , if a valid sample was obtained 
CALLED BY:insPosit (ins) 
insSetup (ins) 

CALLS: Get (packetBuf f er ) 

createSample (sampler) 
formatSample (sampler) 
inLimitSample (sampler) 

//checks for the arrival of a new sample 
Boolean 

sampler: : getSample ( stampedSampleSc newSample) 

{ 



if ( readSamples (newSample) ) { 

filterSample (newSample) ; 
formatSample ( newSample) ; 
return TRUE; 



} 

return FALSE; //Sample packet not available 






obj ect 
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PROGRAM: reade Samples 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 12 May 1996 

FUNCTION: Retrieves all samples of the IMU, water speed, and depth 

that are present in the A2D FIFO until the FIFO is EMPTY. Calculates 
delta_t . 

RETURNS: Boolen ; TRUE - There were new samples pulled from the FIFO 

FALSE - There were no new samples 
CALLED BY : getSample 

CALLS : getFif oStatus ( ) , getFifoData ( ) 

Boolean 

sampler: : readSamples ( stampedSampleSc newSample) 

{ 



//Did the FIFO overflow? 
if (a2dl . getFif oStatus ( ) == FULL) { 
gotoxy (17,20) ; 

cout << "FIFO Overflowed, execution terminated..." « endl; 
exit (1) ; 



//Does the FIFO have new samples? 
if (a2dl . getFif oStatus ( ) != EMPTY) { 

sampleCount = 0; //Counts the number of samples taken 

//Empty the FIFO 

while (a2dl .getFifoStatus ( ) 1= EMPTY) { 



sample [ sampleindex] [ subSampleIndex++ ] = a2dl . getFif oData () ; 

//Has it pulled one sample of each channel from the FIFO? 
if (subSample Index == 8) { 

subSampleIndex= 0; 

increment ( sampleindex) ; //set to record next sample 
+ + s amp 1 eCoun t ; 



if (sampleCount >0) { 

//calculate time delta 

newSample . del taT = sampleCount * samp 1 e Per iod ; 

gotoxy (1,19) ; 
printfC "); 

gotoxy (1,19); 

print f (" sampleCount : %d" , sampleCount ) ; 
return TRUE; 

} 
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} 



else { 

return FALSE; 

} 



else { 

//No new samples 
return FALSE ; 

} 

) 

PROGRAM: createSample 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 15 May 1996 

FUNCTION: Low pass filters by averaging over all samples received since 

the last sample.. 

RETURNS: void 

CALLED BY: getSample 
CALLS : none 

void 

sampler: : f ilterSample ( stampedSampleSc newSample) 

{ 



for (int i = 0; i < 8; i++) { 

newSample . sample [ i ] = 0; 

} 



int j ( sample Index) ; 



for (i = 0; i < sampleCount; i++) { 



} 



decrement ( j ) ; 
newSample . sample [ 0 ] 
newSample . sample [ 1 ] 
newSample . sample [ 2 ] 
newSample . sample [ 3 ] 
newSample . sample [ 4 ] 
newSample . sample [ 5 ] 
newSample . sample [ 6 ] 
newSample . sample [7 ] 



+= sample [ j ] [ 0 ] 
+= sample [ j ] [ 1 ] 
+= sample [ j ] [ 2 ] 
+= sample [ j ] [ 3 ] 
+= sample [ j ] [ 4 ] 
+= sample[ j ] [5] 
+= sample [ j ] [ 6 ] 
+= sample [ j ] [7 ] 



/ sampleCount; 
/ sampleCount; 
/ sampleCount; 
/ sampleCount; 
/ sampleCount; 
/ sampleCount; 
/ sampleCount; 
/ sampleCount; 



} 
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PROGRAM: format Sample 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 15 May 1996 

FUNCTION : Converts integers representing voltage readings into 
units which are useable by the INS. 

RETURNS: void 
CALLED BY:getSample 
CALLS : none 



//Calls the methods to convert the voltages to real world units 
void 

sampler :: formatSample ( stampedSampleSc newSample) 

{ 

newSample . sample [ 0 ] = xAccelUnits{newSample.sample[0]); 
newSample . sample [ 1 ] = yAccelUnits{newSample.sample[l]); 
newSample . sample [ 2 ] = zAccelUnits {newSample . sample [2]); 



newSample . sample [3 ] 
newSample . sample [ 4 ] 
newSample . sample [ 5 ] 



pUnits (newSample. sample [3] ) ; 
qUnits (newSample . sample [ 4] ) ; 
rUnits (newSample . sample [ 5 ] ) ; 



newSample . sample [ 6 ] 
newSample . sample [7 ] 



waterSpeedUnits (newSample . sample [ 6 ] ) ; 
compl .getHeading { ) ; 



96 



L. COMPASS.H 



#ifndef _COMPASS_H 
#define _COMPASS_H 



# include 
#include 
#include 



<iostream . h> 
<f stream. h> 
<conio . h> 



#include 

#include 

#include 

#include 



" por thank. h" 
" toetypes . h” 
" location . h" 
"compBuf f .h" 



CLASS : compass 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

MODIFIED: 15 May, 1996 by Eric Bachmann Sc Randy Walker 
FUNCTION: Reads compass messages from the compass buffer. Checks for 
validchecksum . Corrects heading for megnetic variation. Heading is 
continous. There is no branch cut at 360 degrees. 



class compass { 
public : 

//Class Constructor 

compass ( ) : port2 (COMports . Ini t (COM2 , BYTE (3), b9600, 

NOPARITY, BYTE(8), BYTE ( 1 ) , NONE, headings)) 

{) 

//Initialize currentHeading 
float initCompass ( ) ; 

//returns the latest heading 
float getHeading { ) ; 

private : 

//buffer for compass data 
compBuffer headings; 

//Maintains the most recently obtained heading, 
float currentHeading ; 

//instantiates serial port communications on comm2 
buf f eredSer ialPort& port2; 

//calculates the check sum of the message 
Boolean checkSumCheck (const compDataSc) ; 

//Parses the heading out of a compass message. 
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float parseCompData (const compDataSc, const BYTE) ; 



// Convert magnetic direction based on magnetic variation, 
float trueHeading (const float); 

// Returns the heading without branch cuts 
float continousHeading (const float); 



); 



#endif 



M, COMPASS.CPP 

# include <math.h> 

# include “ compass . h “ 

float 

compass : : initCompass ( ) 

{ 

float tempHeading; 
compData rawMessage; 

while ( (headings .Get (rawMessage) ==FALSE) 

11 (checkSumCheck (rawMessage) ==FALSE) ) {) 

tempHeading = parseCompData ( rawMessage , 'C‘) * degToRad; 

currentHeading = cont inousHeading ( trueHeading ( tempHeading ) ) 

return currentHeading; 

) 
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NAME : getHeading 

AUTHOR; Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: 

Determines if an updated gps position message is available and 
copies it into the input argument ' rawMessage ‘ . If the message 
has a valid checksum and was obtained with atleast three 
satelites in view, a 'TRUE' is returned to the caller, 
indicating that the message is valid. 

RETURNS: TRUE, if a valid position message is contained in the 

input argument. 

CALLED BY: navPosit ( navigator . h) 

CALLS :Get (buffer. h) 

checkSumCheck (gps.h) 



float 

compass : : getHeading ( ) 

{ 

float tempHeading ; 

Boolean checkSumFlag; 
compData rawMessage; 

if { (headings . Get (rawMessage) ) && ( checkSumCheck (rawMessage )) ) { 

tempHeading = parseCompData ( rawMessage , 'C') * degToRad; 

currentHeading = 

continousHeading (trueHeading (tempHeading) ) ; 
return currentHeading; 

} 

else { 

return currentHeading; //No updated position is available. 

} 



) 



BYTE 

asciiToHex (BYTE letter) 

{ 



if ( letter >= ’ A ' ) { 

return (letter - 'A' + 10); 

} 

else { 

return (letter - 48); 

} 

} 
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PROGRAM: checkSumCheck 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: 

Takes an exclusive or of bytes 2 through 78 in a Motorola format (@@EA) 
position message and compares it to the checksum of the message of the 
message . 

RETURNS: TRUE, if the message contains a valid checksum 
CALLED BY: gps Posit ion (gps) 

CALLS; none 



Boolean 

compass :: checkSumCheck ( const compDataSc newMessage) 

{ 

BYTE calChkSum(O) ; 

BYTE mesChkSum(O) ; 

for ( int i = 1 ; newMessage [ i ] != '*•; i++) { 

calChkSum newMessage [ i ] ; 

} 

mesChkSum = asci iToHex ( newMessage [ i+1 ] ) * 16 

+ asciiToHex (newMessage [ i+2 ]) ; 



return Boolean ( calChkSum == mesChkSum) ; 

} 
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PROGRAM 
AUTHOR : 
DATE: 



trueHeading 

Eric Bachmann, Dave Gay 
11 July 1995 



FUNCTION; Convert magnetic direction to true based on local magnetic 
variation . 

RETURNS: true heading 

CALLED BY: insPosit 

insSetUp 

CALLS : none 

float 

compass :: trueHeading {const float magHeading) 

{ 

static double twoPi(2.0 * M_PI); 

double trueHeading = magHeading + RADIANMAGVAR; 

if (trueHeading > twoPi) { 
trueHeading -= twoPi; 

) 

return trueHeading; 



} 
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PROGRAM: cont inousHeading 

AUTHOR: Eric Bachmann 



DATE : 
FUNCTION: 
RETURNS : 
CALLED BY: 

CALLS : 



11 July 1995 

Maintains track of branch cuts and returns a continous heading, 
continous true heading 
insPosit 
insSetUp 
none 






float 

compass :: cont inousHeading (const float trueHeading) 

{ 

const float twoPi(2.0 * M_PI); 

static int branchCutCount ( 0 ) ; 

static float previousHeading ( trueHeading) ; 

if ((4.71 < previousHeading) && (trueHeading < 1.57) ){ 

++branchCutCount ; //Went through North in a right hand turn 

} 

else { 

if ((1.57 > previousHeading) ScSc (trueHeading > 4.71)) { 

““branchCutCount ;/ /Went through North in a left hand turn 

} 

) 



previousHeading = trueHeading; 

return trueHeading + (branchCutCount * twoPi) ; 
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PROGRAM: parseHeading 

AUTHOR: Eric Bachmann 

DATE: 11 July 1995 

FUNCTION: Parses the heading out of a compass message. 

RETURNS: the message heading as a float 

CALLED BY: insPosit 

insSetUp 

CALLS : none 

float 

compass : :parseCompDat a (const compDataSc rawMessage, const BYTE key) 

{ 

float dataSum(O) ; 

for(int j = 0; rawMessage [ j ] i= key; j++){) 

j++; 

for(int i = 0; rawMessage [i + j] != i++){) 

switch (i) { 

case 3 : 



( rawMessage 


[j] - 


48) * 


100.0 + 


( rawMessage 


[j+i] 


- 48) 


* 10.0 + 


( rawMessage 


[ j+2] 


- 48) 


+ 


( rawMessage 


[j + 4] 


- 48) 


* 0.1; 



break; 
case 2 : 

dataSum = ( rawMessage [ j ] - 48) * 10.0 + 
(rawMessage [ j+1 ] - 48) + 

( rawMessage [ j +3 ] - 48) * 0.1; 

break; 
case 1 : 

dataSum = ( rawMessage [ j ] - 48) + 

(rawMessage [j +2 ] - 48) * 0.1; 

break; 



return dataSum; 
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N. A2D.H 



#ifndef A2D_H 

#define A2D_H 



#include 

#include 

#include 

#include 

#include 

#include 

#include 

#include 



<dos .h> 

<math . h> 
<conio -h> 
<stdio . h> 
<stdlib . h> 
<stdarg .h> 
<iostream . h> 
<fstream . h> 



//ESP A2D General Global Definitions 



#define DEFEASE 0x100 
#define FIFOSIZE 1000 
#define MAXCHAN 0x10 



//Base address SEL=l->0x300 Sc 
//FIFO size (MAX=1000 decimal 
//Max channels 



SEL=0->0xl00 



//ESP A2D Status Register Definitions 
//BASE+02h: 01 ID DDDD 

#define INT_STAT 0x10 // 0001 0000 INTERRUPT STATUS 

#define TRG_STAT 0x08 // 0000 1000 TRIGGER STATUS 



(1=IRQ Pending) 
( l=Triggered) 



#define FULL 0x01 
#define HALF 0x05 
#define EMPTY 0x06 



// 0000 0001 FIFO FULL (001=Full) 

// 0000 0101 FIFO HALF FULL (101=Half Full) 

// 0000 0110 FIFO EMPTY (110=Empty) 



//ESP A2D Control Register Definitions 
//BASE+08h: DDDD DDDD 
//BASE+09h; DDDD DDRR 

#define GATElOUT 0x0008 // 0000 0000 0000 1000 GATElOUT (Always Driven) 



#def ine 


TRG_POS 


0x0010 // 


0000 


0000 


0001 


0000 


TRIG 


i POS 


(Trig on + I - 


■) 


#def ine 


SET_TRG 


0x0020 // 


0000 


0000 


0010 


0000 


TRIG 


; SET 


(Active LOW) 


#def ine 


RST_TRG 


0x0040 // 


0000 


0000 


0100 


0000 


TRIG 


: CLR 


(Active LOW) 


#def ine 


INT_EN 


0x0080 // 


0000 


0000 


1000 


0000 


IRQ 


ENAB 


(Active HIGH) 


#def ine 


DIFF 


0x0400 


// 


0000 


0100 


0000 


0000 


DIFF/SE 


(1=DIFF 


0=SE) 


#def ine 


RMS 


0x0800 


// 


0000 


1000 


0000 


0000 


RMS Mode 


(l=ON 


0=OFF) 


#def ine 


CAL 


0x1000 


// 


0001 


0000 


0000 


0000 


CAL 


Mode 


(l=ON 


0=OFF) 


#def ine 


PRG^SEQ 


0x1000 


// 


0001 


0000 


0000 


0000 


SEQ 


Mode 


(1=PRG 


0=RUN) 


#def ine 


ACDC 


0x2000 


// 


0010 


0000 


0000 


0000 


ACDC 


Mode 


(1=DC 


0=AC) 


#def ine 


SAM__SEQ 


0x4000 


// 


0100 


0000 


0000 


0000 


SAMP/SEQ 


(1=SEQ 


0=SAMP) 


#def ine 


RST_FIFO 


0x8000 


// 


1000 


0000 


0000 


0000 


FIFO 


Reset ( 1=EN 


0=REW) 



//ESP A2D Useful Definitions 

#define CLRRATE 0xFFF8 // CLEAR RATE TO HIGHEST RATE 
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// CLASS NAME; a2dClass 
// AUTHOR: Randy Walker 
// DATE; 27 March 1996 

// DESCRIPTION: Provides for the software operation of the A2D module. 

//Class Definition for the A2D Class 
class a2dClass { 

public : 

//Class Constructor; reads a2d.cfg file, initializes hardware 
a2dClass ( ) ; 

//Reads a2d.cfg configuration file 
void readConf igFile ( ) ; 

//Sets address mapping 
void initSysAddr ( void) ; 

//Initializes the A2D Control Register 
void initHardware ( void) ; 

//Print out the variable ctrlw, for debug purposes 
void printCtrlw ( void) ; 

//Sets the A2D Control Register for Single-Ended mode 
void setSe (void) ; 

//Sets the A2D Control Register for Differential mode 
void setDif f (void) ; 

//Loads sequencer memory with channel data 

void setChannel (unsigned seq, unsigned ch, unsigned glO, unsigned g2) 

//Sets sequencer to program mode 
void setProgSeq ( void) ; 

//Sets sequencer to run mode 
void setRunSeq ( void) ; 

//Loads sequencer address counter with number of channels to scan, 
void setCount (unsigned nch) ; 

//Sets AC or DC Coupling 
void setAcDc (unsigned acdc) ; 

//Prevents triggering 
void lockTrigger (void) ; 

//Allow the triger to function 
void unlockTrigger (void) ; 
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//Toggle the trigger (software triggering) 
void setTr igger ( void) ; 

//Clears the trigger 
void resetTr igger (void); 



//Switches in the RMS measurement chip 
void setRmsOn (void) ; 

//Switches out RMS measurement chip 
void setRmsOf f (void) ; 

//Sets the A2D module to sequencer mode 
void setSequencer (void) ; 

//Sets the A2D module to sampler mode 
void setSamplerRate (unsigned) ; 

//Set GATEIOUT bit of control word high 
void gateloutOn ( void) ; 

//Set GATEIOUT bit of control word low 
void gateloutOf f ( void) ; 

//Sets timer channel 1 to square-wave input 
void squareWaveTimerl (unsigned) ; 

//Initialize the A2D timing using timer 2 
void initTiming (unsigned dt); 

//Rewind FIFO to beginning of memory 
void resetFif o ( void) ; 

//Enable FIFO to acquire data 
void setFifo ( void) ; 

//Returns th state of the fifo 
unsigned getFifoStatus (void) ; 

//Returns next data word stored in FIFO 
signed getFif oData ( void) ; 

//Program timer channel 0 to set the desired interrupt rate 
void setIntRate (unsigned intrate) ; 

//Locksout the interupt request line 
void intOf f ( void) ; 

//Enables system interuppt request 
void intOn(void); 

//Sets the trigger level; trigger level (0=-10V, 128=0V, 255=+10V) 
void setTriggerLevel (unsigned tl) ; 
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//Sets falling or rising edge trigger 
void setTriggerPosition (unsigned tp) ; 

//Calibrates zero offset error 
void zeroOf f set (void) ; 



//Grounds the two differential inputs for zero adjust 
void grndinput ( void) ; 

//Ungrounds the two differential inputs 
void freeinput (void) ; 

//Adjust the trimmer on the PGA 
void zeroAdjust (void) ; 



int Ghent; //Number of channels to sequence 

unsigned delta_t; //period between channels 



private : 



unsigned 

unsigned 

unsigned 

unsigned 

unsigned 

unsigned 

unsigned 

unsigned 

unsigned 

unsigned 



ctrlw; 
seqent ; 
mode_sel ; 
mode_acdc ; 
samprate ; 
sampindex; 
seqaddr [MAXCHAN] ; 
chan [MAXCHAN] ; 
glO [MAXCHAN] ; 
g2 [MAXCHAN] ; 



//Holds A2D Control Register update values 
//Sequence Counter 
//Single-ended or Differential 
//AC/DC Coupling 

//Sample Rate in Recurrent Mode 

//Which Channel to Sample in Recurrent Mode 

//Sequencer Address 

//Channel 

//xlO Gain 

//x2 Gain 



}; 



#endif 



O. A2D.CPP 

# include "a2d.h" 



//ESP A2D Addresses 



unsigned 


BASE 


= 


DEFBASE ; 


// 


BASE I/O ADDR 


[BASE] 


0 


unsigned 


FIFO 


= 


0x0 0 ; 


// 


FIFO READ ADDR 


[00-01] 


(R) 


unsigned 


MEM 


= 


0x0 0 ; 


// 


SEQUENCER ADDR 


[00-01] 


(W) 


unsigned 


STAT 


= 


0x02 ; 


// 


STATUS REGISTER 


[02] 


(R) 


unsigned 


COUNT 


= 


0x02 ; 


// 


SEQUENCER ADDR PTR 


[02] 


(W) 


unsigned 


TIMERO 


= 


0x04 ; 


// 


TIMER 0 


[04] 


(R/W) 


unsigned 


TIMERl 


= 


0x0 5; 


// 


TIMER 1 


[05] 


(R/W) 


unsigned 


TIMER2 


= 


0x0 6 ; 


// 


TIMER 2 


[06] 


(R/W) 


unsigned 


TIMERC 


= 


0x07 ; 


// 


TIMER CONTROL WORD 


[07] 


(R/W) 


unsigned 


CNTL 


= 


0x08; 


// 


A2D CONTROL REGISTER 


[08-09] 


(W) 


unsigned 


DAC 


= 


OxOC ; 


// 


DAC DATA 


[OC] 


(W) 
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// FUNCTION NAME: a2dClass() 

// AUTHOR: Randy Walker 
// DATE: 27 March 1996 

// DESCRIPTION: Sets defaults, reads a2d.cfg file, initializes address map 
/ / and hardware 

// RETURNS: void 

// CALLS: readConf igFile ( ) , initSysAddr ( ) , initHardware ( ) 

// CALLED BY: Object declaration 

! l-k-k-k-k-k-if-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k'k-k-k'k’k'k-k'k'k-k'k-k-k'k'k'k'k-k-k'k'k-k-k-k'kieic'k-k-k-k-k-k-ic’k-k-k-k-k-k-k-k-k-k-k-k-k-k 

a2dClass : :a2dClass (void) 

{ 

ctrlw=0 ; 
seqcnt=l ; 
mode_sel=0 ; 
mode_acdc=l ; 
delta__t = 3 ; 
chcnt=l ; 
samprate=0 ; 
sampindex=0 ; 
readConf igFile ( ) ; 
initSysAddr ( ) ; 
initHardware ( ) ; 



// FUNCTION NAME: readConf igFi le ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Reads the a2d.cfg file and sets variables 
// RETURNS: void 
/ / CALLS : none 

// CALLED BY: a2d class constructor 

j I’k'k'k’k’k’k'k'k’k'k'k'k'k'k'k'k'k’k’k'k'k'k'k'k’k'k'kit'k'k'k'k'k'k-k'k'kit'k'k’k’k'k'k'k'k’k'if’k'k'k'k'k’k'k'k'k'k’k'k'k'k'k'k'k'k'k'k'k 

void a2dClass :: readConf igFile ( ) 

{ 

FILE *conf igFile; 
char junk [ 128 ] ; 

if ( (conf igFile = f open ( "a2d . cf g" , "r")) == NULL) { 
fprintf { stderr , "Cannot open file A2D . CFG . . . \n " ) ; 
exit ( 1 ) ; 

} 

f scanf (conf igFile , " %x%s " , Scseqcnt , junk) ; 

if (seqcnt==0 I | seqcnt>OxOF) { //seqcnt must be 1-F (15 max in seq mode) 
cout « "\nseqcnt out of range in A2D . CFG . . . \n" ; 
exit ( 1 ) ; 

} 

f scanf (conf igFile, " %d%s" , Scmode_sel , junk) ; 
if (mode_sel !=0 && mode_sel != 1){ 

cout « "\nmode_sel out of range in A2D . CFG . . . \n " ; 
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exit ( 1) ; 

} 

f scant (conf igFile, ''%d%s" , Scinode_acdc , junk) ; 
if (inode_acdc !=0 && mode_acdc != 1) { 

cout << " \nmode_acdc out of range in A2D . CFG . . , \n " ; 
exit (1) ; 



f scant (conf igFile , " %x%s " , &chcnt , junk) ; 

if (chcnt == 0 I I chcnt > OxOF) { //chcnt must be 1-F (15 max in seq mode) 
cout << "\nchcnt out of range in A2D . CFG . . . \n“ ; 
exit (1) ; 



f scant (conf igFile , " %d%s " , &delta_t , junk) ; 
if (delta_t <3 11 delta_t > 8192) ( 

cout « "\ndelta_t out of range in A2D . CFG . . . \n " ; 
exit ( 1 ) ; 



if (delta_t < 6 && chcnt > 1) ( 

cout « "\ndelta_t must be > 6 for chcnt > l...\n"; 
exit ( 1 ) ; 



f scant (conf igFile , " %d%s " , &samprate , junk) ; 
if (samprate > 7){ 

cout « "\nsamprate out of range in A2D . CFG . . . \n " ; 
exit (1) ; 



fscanf (conf igFile, "%x%s'' , Scsampindex, junk) ; 
if (sampindex > OxOF) { 

cout « "\nsampindex out of range in A2D . CFG . . . \n " ; 
exit (1) ; 

) 

for (int i=0 ; i<seqcnt ; i++ ) 

fscanf ( conf igFile, " %x%x%x%x" , &seqaddr [ i ] , &chan [ i ] , &gl0 [ i ] , &g2 [ i ] ) ; 
fclose (conf igFile) ; 



) 
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j j'k-ic-k'it'it-k'ifif-k-k-k-k-k-k'if-k'k-k-k-k-k-k-k-k-k-k-k'k'k-k'lf'if-k-k-k'k-k-k’k'k-k-k-k-k-k-k-k-k-k-k'k-k-k'k-k-it-k’k-k-k-k-k-k-k-k-k-k-k-k 

// FUNCTION NAME: initSysAddr ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets system address mappings 
// RETURNS: void 
// CALLS: none 

// CALLED BY: a2d class constructor 

j l-k-k-i(-k-*;-k-k-k-k-k-k-k-k-k-k-k-k-if-if-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k'k-k-k-k-k-k-k-k-i(-k-k-k-k-k-k-k-k-k-k-k’k-k-if-k-k-k-k-k‘k-k-k 

void a2dClass: : initSysAddr (void) 



/ /clear 


BASE 










FIFO 


£c — 


OxOF; 


// 


FIFO READ ADDRESS 


[00,01] 


(R) 


MEM 


& = 


OxOF; 


// 


SEQENCER MEM ADDRESS 


[00,01] 


(W) 


STAT 


Sc — 


OxOF; 


// 


STATUS REGISTER 


[02] 


(R) 


COUNT 


Sc — 


OxOF; 


// 


SEQENCER ADDRESS PTR 


[02] 


(W) 


TIMERO 


Sc — 


OxOF; 


// 


TIMER 0 


[04] 


(R/W) 


TIMERl 


Sc — 


OxOF; 


// 


TIMER 1 


[05] 


(R/W) 


TIMER2 


Sc — 


OxOF; 


// 


TIMER 2 


[06] 


(R/W) 


TIMERC 


Sc = 


OxOF; 


// 


TIMER CONTROL WORD 


[07] 


(R/W) 


CNTL 


Sc — 


OxOF; 


// 


CONTROL REGISTER 


[08] 


(R/W) 


DAC 


Sc — 


OxOF; 


// 


DAC DATA 


[OC] 


(W) 


//set BASE 












FIFO 


1 = 


BASE ; 


// 


FIFO READ ADDRESS 


1 

o 

o 

o 


(R) 


MEM 


1 = 


BASE ; 


// 


SEQENCER MEM ADDRESS 


[00,01] 


(W) 


STAT 


1 = 


BASE; 


// 


STATUS REGISTER 


[02] 


(R) 


COUNT 


1 = 


BASE ; 


// 


SEQENCER ADDRESS PTR 


[02] 


(W) 


TIMERO 


1 = 


BASE ; 


// 


TIMER 0 


[04] 


(R/W) 


TIMERl 


1 = 


BASE ; 


// 


TIMER 1 


[05] 


(R/W) 


TIMER2 


1 = 


BASE ; 


// 


TIMER 2 


[06] 


(R/W) 


TIMERC 


1 = 


BASE ; 


// 


TIMER CONTROL WORD 


[07] 


(R/W) 


CNTL 


1 = 


BASE; 


// 


CONTROL REGISTER 


[08] 


(R/W) 


DAC 


1 = 


BASE; 


// 


DAC DATA 


[OC] 


(W) 



) 
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// FUNCTION NAME: initHardware ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the A2D Control Register to 0020 and sets the 

// data member, ctrlw=0060; initializes the module setup 

// for software triggering of the A2D. Programs each 

/ / channel . 

// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: a2d class constructor 

void a2dClass : : initHardware (void) 

{ 

outpw (CNTL, SET_TRG) ; 
ctrlw = SET_TRG|RST_TRG; 

if (mode_sel == 0) 
setSe ( ) ; 

else 

setDif f ( ) ; 

for(int i = 0;i < chcnt;i++){ 

setChannel ( seqaddr [ i ] , chan [ i ] , glO [ i ] , g2 [ i ] ) ; 

} 

setAcDc (mode_acdc) ; 
initTiming (delta_t ) ; 
setCount (chcnt ) ; 



// FUNCTION NAME: printCtrlw() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Print A2D control register var, ctrlw. 

// The variable is used to set a byte in the 

// ESP A2D control register at BASE + 08h/09h 

// Used during application code debug 

// RETURNS: void 
/ / CALLS : none 
// CALLED BY: none 

void a2dClass : ; pr intCtr Iw (void) 

{ 

printf ( "ctrlw: %04x\t", ctrlw); 
for (int i=0x00 ; i<0xl0 ; i++) { 

printf (" %i ",(( ctrlw»0x0F-i ) & 1)); 
if ((i+l)%4==0) 
printf (" "); 

) 



) 
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// FUNCTION NAME: setSe () 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets ctrlw for single ended mode and writes ctrlw to 
// A2D Control Register 

// RETURNS: void 
// CALLS: outpw() 

// CALLED BY: initHardware () 

void a2dClass : : setSe ( void) 

{ 

ctrlw -DIFF; 
outpw(CNTL, ctrlw) ; 



// FUNCTION NAME: setDiffO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets ctrlw for differential mode and writes ctrlw to 
// A2D Control Register 

// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: initHardware ( ) 

void a2dClass : : setDif f ( void) 

{ 

ctrlw 1= DIFF; 
outpw (CNTL, ctrlw) ; 



} 
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// FUNCTION NAME: setChannel ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Loads sequencer memory with channel data 
// CALLS: progSeq(), outpw ( ) , runSeq ( ) 

// CALLED BY: ini t Hardware ( ) 

// VARIABLES: seq - sequencer number 
// ch - channel number 

// glO - xlO gain value 

// g2 - x2 gain value 

void a2dClass: : setChannel (unsigned seq, unsigned ch, unsigned glO, unsigned g2) 

{ 

unsigned d = 0; 



setProgSeq ( ) ; 
outpw (COUNT, seq) ; 



// set sequencer program mode 
// set sequencer address 



//load sequencer memory 
d I = ch<<8 ; 
d I = (g2«12 ) ; 
d 1= (gl0«14); 
outpw (MEM, d) ; 



// 


channel 


// 


gain 


X2 


// 


gain 


XlO 


// 


load 


sequencer 



setRunSeq ( ) ; 



// set sequencer run mode 



) 



// FUNCTION NAME: setProgSegO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets sequencer to program mode 
// RETURNS: void 
// CALLS: outpw () 

/ / CALLED BY : setChannel ( ) 

void a2dClass : : setProgSeq ( void) 

{ 

ctrlw 1= PRG_SEQ; 
outpw (CNTL,ctrlw); 



} 
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j y********************************************************************* 

// FUNCTION NAME: setRunSeqO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets sequencer to run mode 
// RETURNS: void 
// CALLS: outpwO 
// CALLED BY: none 

! |•k•k•k•k•k'k•k•k•k•k•k•k•k•k•k^k•k'k'k•k'k'k•k'k'k■if'k'k'k•k’k•k'k'it'k'k'k'k'k'it•k'k'k•k•k•k•k•k^k•k•k•k•k•k•k•k•k•i(•k•k'k•k•k•k•k•k•k•k•k 

void a2dClass : : setRunSeq (void) 

{ 

ctrlw &= -PRG_SEQ; 
outpw (CNTL, ctrlw) ; 



// FUNCTION NAME: setCount ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Loads sequencer address counter with number of channels 
// to scan. 

// RETURNS: void 

// CALLS: outpw(), set ProgSeq ( ) , setRunSeq() 

// CALLED BY: initHardware ( ) 

// VARIABLES: nch - number of channels to sequence 

void a2dClass :: setCount (unsigned nch) 

{ 



nch=nch<<4 ; 


// 


put 


in 


upper nibble 


outpw (COUNT, nch) ; 


// 


out 


to 


register 


setProgSeq ( ) ; 


// 


reset 


sequencer 


setRunSeq ( ) ; 


// 


put 


it 


in run mode 



} 

j l‘k'k'k‘k‘k'k'k‘k'k'k‘k'k'k'k'k‘k'k'k'^'k'k'ic'if'k'k'k'k'^'if’k'k'if'^'i(’k'k'k'it'k'k'k'k'k'k'k'k'k’k'k'k'k‘k'k’k‘k'k'k'k'k’k'k'k'k'k'k'k'k'k'k 

// FUNCTION NAME: setAcDc ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets AC or DC Coupling 
// RETURNS: void 
// CALLS: outpw() 

// CALLED BY: initHardware ( ) 

// VARIABLES: acdc - holds coupling value 

void a2dClass :: set AcDc (unsigned acdc) 

{ 

if (acdc) 



ctrlw 1= ACDC; 


// 


acdc=l -> DC 


else 






ctrlw -ACDC; 


// 


acdc=0 -> AC 


outpw (CNTL, ctrlw) ; 







) 
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// FUNCTION NAME: lockTrigger ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 
// DESCRIPTION: Prevents triggering 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: initSampler ( ) 

void a2dClass :: lockTrigger (void) 

{ 

ctrlw &= ~RST_TRG; 
outpw(CNTL, ctrlw) ; 



// FUNCTION NAME: unlockTrigger ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 
// DATE: 27 March 1996 

// DESCRIPTION: Allow the trigger to function 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: readSmaple ( ) 

void a2dClass : : unlockTrigger (void) 

{ 

ctrlw 1= RST_TRG |SET_TRG; 
outpw(CNTL, ctrlw) ; 



I ^***********************************-*********-*r*******-*r**************** 

// FUNCTION NAME: setTrigger ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Toggle the trigger (software triggering) 

// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY:readSample( ) 

void a2dClass :: setTrigger (void) 

{ 

outpw ( CNTL , ctr lw&~SET_TRG I RST_TRG ) ; 
outpw(CNTL, ctrlw I SET_TRG I RST_TRG) ; 



) 



115 



// FUNCTION NAME: resetTrigger ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Clears the trigger 

// RETURNS: void 

// CALLS: outpwO 

// CALLED BY: readSample ( ) 

void a2dClass: : resetTrigger (void) 

{ 

outpw(CNTL, Ctrlw I SET_TRG&~RST_TRG) ; 
outpw ( CNTL , ct r Iw I SET_TRG I RST_TRG ) ; 



! y********************************************************************* 

// FUNCTION NAME: setRmsOnO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Switches in the RMS measurement chip 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

void a2dClass ; : setRmsOn ( void) 

{ 

ctrlw i= RMS; 
outpw (CNTL, ctrlw) ; 



! y************^*********************************************Tlr********** 

// FUNCTION NAME: setRmsOffO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Switches out RMS measurement chip 
// RETURNS: void 
// CALLS: outpw () 

// CALLED BY: initSampler ( ) 

I l-k-k-k'k-k-k-k'k'k'k'k'k'k'k'k'k'k'k'k'k'k'k-k-k'k-k-k-k-k-k-k'k'k-k-k-k'k-k-k'k-k-k-k'k-k-k-k-i^’k'k'k’k'k-k’k-k’k-k-k-k-k-k-k-k-k-k-k-k-k 

void a2dClass : : setRmsOf f ( void) 

{ 

ctrlw Sc= ~RMS; 
outpw (CNTL, ctrlw) ; 



) 
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// FUNCTION NAME: setSequencer ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the A2D module to sequencer mode 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

y'y********************************************************************* 
void a2dClass : : setSequencer (void) 

{ 

ctrlw 1= SAM_SEQ; 
outpw ( CNTL, ctrlw) ; 



// FUNCTION NAME: setSamplerRate ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the A2D module to sampler mode 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

// VARIABLES; rate - sampler rate 



void a2dClass :: setSamplerRate (unsigned rate) 



ctrlw Sc= --SAM_SEQ; 
ctrlw &= CLRRATE; 
ctrlw 1= rate; 
outpw (CNTL, ctrlw) ; 



//Set to sampler mode 
//Clear previous rate 
//Set new rate 
//Set Control Word 



to 00 0 



// FUNCTION NAME: gateloutOn ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Set GATEIOUT bit of control word high 
// RETURNS: void 
// CALLS: outpw () 

// CALLED BY: none 

void a2dClass :: gateloutOn (void) 

{ 

ctrlw 1= GATEIOUT; 
outpw (CNTL, ctrlw) ; 



} 
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// FUNCTION NAME: gateloutOf f ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Set GATEIOUT bit of control word low 
// RETURNS: void 
// CALLS: outpwO 
/ / CALLED BY : none 

j y***-*--*-******************************************************-*-****-*'**** 

void a2dClass; : gateloutOf f (void) 

{ 

Ctrlw &= -GATEIOUT; 
outpw(CNTL, ctrlw) ; 



} 

// FUNCTION NAME: squareWaveTimerl ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets timer channel 1 to square-wave input 
// RETURNS: void 
// CALLS: outpO 
// CALLED BY: none 

// VARIABLES: dt-micro seconds per period (1 to 8192) 

// assuming 8 MHz clock input 

// ch- timer channel 1 

// ph- local variable 

// pl-local variable 

I 

void a2dClass :: squareWaveTimerl (unsigned dt) 

{ 

char ph,pl; 



pi = (dt*8)&0xFF; 


// 


8 CLOCKS PER uS 


ph = (dt*8)>>8; 






outp ( TIMERC , 0x7 6 ) ; 


// 


initialize timer 


outp(TIMERl,pl) ; 


// 


dt uS delay 


oUtp(TIMERl,ph) ; 


// 


with 8 MHz clock 



} 
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// FUNCTION NAME: initTimingO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Initialize the A2D timing using timer 2 
// RETURNS: void 
// CALLS: outpO 
// CALLED BY: initHardware ( ) 

// VARIABLES: dt - number of micro seconds (3 to 2730) 

void a2dClass :: initTiming (unsigned dt) 

{ 



char ph,pl; 






pi = (dt*8)&0xFF; 


// 


8 CLOCKS PER uS 


ph = (dt*8)»8; 






outp(TIMERC, 0xB6) ; 


// 


initialize timer2 


outp(TIMER2,pl) ; 


// 


dt uS delay 


outp (TIMER2 , ph) ; 


// 


with 8 MHz clock 



I l-k-k-ii-ie’k-if-k-if’k'k-k-k'k-k’k-k-k’k'k-k'k-k-k-k-k-k-k-k'k-k'k-k-ii-k-k’k-k-k-k-k-k-k-k'k-k'k-k'k-k'k-k-k'k'k-k'k'k-k-k'k-k’k'k-k-k’k-k-k-k 

II FUNCTION NAME: resetFifoO 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Rewind FIFO to beginning of memory 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

void a2dClass : : resetFifo ( void) 

{ 

ctrlw Sc= --RST_FIFO; 
outpw ( CNTL, ctrlw) ; 



j j'k'k'k'k'k'k'k-k'k'k-k'k-k-k'k-k-k'k'k-k-k'k-k'k'k-k'k’k'k'ic'if'if'it'k'k'k'k'it'k'k'k'k-k'k'k'k'k'k-k-k'k'k'k-k'k'k'k'k'k'k-k’k'k'k'k'k-k'k-k 

II FUNCTION NAME: setFifoO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Enable FIFO to acquire data 
// RETURNS: void 
// CALLS: outpw () 

// CALLED BY: initSampler ( ) 

j l-k-k’k-k-k-k-k-k-k-k-k'k-k-k-k-k-k-k-k-k-k-k-k'k-k-k-k-k-k-k-k'ic-k'k'k'k'k-k'k-k-k'k’k’k'k'k’k'ic-k'k'k'if’k-k'k'k'k-k’k’k'k-k'it-k-k'k'k-k-k 

void a2dClass : : setFifo (void) 

{ 

ctrlw 1= RST_FIFO; 
outpw(CNTL, ctrlw) ; 



} 
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// FUNCTION NAME: getFi foStatus ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 
// DESCRIPTION: Returns FIFO status 
// RETURNS: RETURNS: 6 - empty 

// 5 - half full 

// 1 - full 

// CALLS: inpwO 
/ / CALLED BY : readSample ( ) 

//★★★★*★★*★*★**★★★*★*★*★★★*★**★★★★***★★★★★★*★★******★* 






★★★★*★★**★★*★★*** 



unsigned a2dClass: : getFifoStatus (void) 

{ 

return { inpw (STAT) &7 ) ; 



) 



// 

// 

// 

// 

// 

// 

// 

// 

// 






★★★★★★**★★★★★*★***★****** 



FUNCTION NAME: getFif oData ( ) 

AUTHOR: Randy Walker, based on [MAXUS 95] 

DATE: 27 March 1996 

DESCRIPTION: Returns next data word stored in FIFO 
RETURNS: 16bits of data. Lower 12 are A2D data 
CALLS : inpw ( ) 

CALLED BY : readSample ( ) 



********************* 



***************************************1r1,ir.k***** 



signed a2dClass: :getFifoData (void) 

{ 

return ( inpw (FIFO) &0x0FFF) ; //Get data and mask upper nibble 



} 



//************************************************i,irir**i,**i,i,t****-l,***** 

1 1 FUNCTION NAME: setIntRateO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Program timer channel 0 to set the desired interrupt 
/ / rate 

// RETURNS: void 
// CALLS: outpO 
// CALLED BY: none 

// VARIABLES: intrate-micro secs per period (1 to 8192) 

// assuming 8 MHz clock input 

void a2dClass :: setIntRate (unsigned intrate) 



{ 



outp(TIMERC, 0x36) ; 



// Set timer 0 to mode 3 



outp(TIMER0, (intrate*8)&0xFF) ; // Load Least Significant Byte 

outp(TIMER0, ( intrate*8 ) >>8 ) ; // Load Most Significant Byte 



) 
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// FUNCTION NAME: intOffO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Locksout the interupt request line 
// RETURNS: void 
// CALLS: outpwO 
// CALLED BY: none 

void a2dClass : : intOf f ( void) 

{ 

ctrlw Sc= -INT_EN; // INT_EN is active high 

outpw (CNTL, ctrlw) ; 



yy********************************************************************* 
// FUNCTION NAME: intOn ( ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Enables system interuppt request 
// RETURNS: void 
// CALLS: outpwO 
// CALLED BY: none 

j y********************************************************************* 

void a2dClass : : intOn (void) 

{ 

ctrlw 1= INT_EN; // INT_EN is active high 

outpw (CNTL, ctrlw) ; 



// FUNCTION NAME: setTriggerLevel { ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the trigger level 
// RETURNS: void 
// CALLS: outpO 
// CALLED BY: none 

// VARIABLES: tl-trigger level (0=-10V, 128=0V, 255=+10V) 

void a2dClass :: setTriggerLevel (unsigned tl) 

{ 

outp (DAC, tl ) ; 

} 
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// FUNCTION NAME: setTriggerPosition { ) 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Sets falling or rising edge trigger 
// RETURNS: void 
// CALLS: outpwO 
// CALLED BY: none 

// VARIABLES: tp : 0=f ailing, l=rising 

j |^k•k'k•k'k'k•k'k'i('tt•k'k•k■k■k'k'k•k•if•k•k•k•k■k'k•k^k'k•k•k•k•k•k•k•k•k■k•k•k•k•k^k■k•k■k•k•k•k•k■k■k•k•k•k•k•k•k•k•k■k'k•k•k'k■k•k•k•k■k 

void a2dClass :: setTriggerPosit ion (unsigned tp) 

{ 

ctrlw £c= ~TRG_POS; //Clear previous TRG_POS 

ctrlw 1= ( tp) ?TRG_POS : 0 ; //Evaluate tp and set ctrlw 
outp (CNTL, ctrlw) ; 



j j’k'k'k'k’if’k'k-k'k'k'k'k'k'k'k'k'k’k’it’k'k'k’it'k'k’k'k’k’k'k'k’it'ic'k'k-k’k'k'k'k-k’k’k'k'k’k-k'k'k’k'k’k'k’k'k'k'k’k'k'k'k'k'k’k’k'k'k'k'k 

// FUNCTION NAME: zeroOffsetO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Calibrates zero offset error 
// RETURNS: void 
// CALLS: outpwO 
// CALLED BY: none 

I j-k'k'k-k'k-k'k'k-k'k-k'k'k-k'if'k'k'k’k'k’k'k'k-k'k’k-i^'k'k'k'k’k-k'k'k'k’k’k-k’k-k'k-k-k'k-k-k-k'k'k-k-k-k-k-k-k'k'k'k-k'k'k'k'k’k’k'k'k'k 

void a2dClass :: zeroOff set (void) 

{ 

unsigned d=0 , i , g2 , gl 0 ; 
float sum; 

float of f setErr [ 4 ] [ 4 ] ; 

float bits[4][4]; 

unsigned gainsl0[4] = {1,10,100,100); 
unsigned gains2[4] = {1, 2, 4, 8); 

clrscr ( ) ; 

printf ( "\n\tG10\tG2\t OFFSET\t\t BITS"); 

for (gl0=0 ;gl0<4 ;gl0++) 
for (g2=0 ;g2<4 ;g2++) 

printf ( "\n\t%d\t%d\t+X.XXXXXX\t+XX.X" ,gl0, g2) ; 

setRmsOf f ( ) ; 
setAcDc ( 0 ) ; 
setSequencer ( ) ; 
initTiming ( 3 ) ; 
setChannel (0,0, glO , g2 ) ; 
grndinput ( ) ; 

delay(5); //Let new gain values stabilize 

while ( i kbhit ( ) ) { 

for (gl0=0 ;gl0<4 ;gl0++) ( 
for (g2=0;g2<4;g2++) { 
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setChannel (0,0, glO , g2 ) ; 
grndinput ( ) ; 
lockTrigger ( ) ; 
resetFifo ( ) ; 
setFifo ( ) ; 
unlockTrigger ( ) ; 
setTrigger ( ) ; 
delay (1) ; 

whi le (getFi f oStatus ( ) i =FULL) ; 
lockTrigger ( ) ; 

for ( i=0 , sum=0 . 0 ; i<FIFOSIZE ; ) { 
d=getFif oData ( ) ; 
sum+= ( float) d* 10/ 2048 ; 

} 

of f setErr [glO ] [g2 ] = ( (float ) ( sum/FIFOSIZE) -10) / 

(float) (gains 10 [glO] *gains2 [g2] ) 

bits[gl0] [g2]=(float) (of f setErr [glO ] [g2]*4096/ 

20*gainsl0 [glO] *gains2 [g2] ) 

} 

clrscr ( ) ; 

printf ( "\n\tG10\tG2\t OFFSET\t\t BITS"); 
for (gl0=0 ; gl0<4 ; gl0++ ) { 
for (g2=0;g2<4;g2++) { 

printf ( " \n\t%d\t%d\t%+l . 6f \t%+04 . If " , glO , g2, 
of f setErr [glO] [g2] ,bits [glO] [g2] ) ; 

) 

) 

} 

f reeinput ( ) ; 
getch ( ) ; 



} 



// FUNCTION NAME: grndinput ( ) 

// AUTHOR; Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Grounds the two diff input for zero adjust 
// RETURNS; void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

void a2dClass : .-grndinput (void) 

{ 

ctrlw 1= CAL; 
outpw (CNTL, ctrlw) ; 
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j l'ki(-k-kirif-k-kif-ki(-k-k-kit-k-k-k-k-k-k-k-k-k-k'k-k-k-k-k-k-kii:if-ic'k-k-k-ki<’k-k-k'k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-i(-k-k-k-k-k'k-k-k-k-k 

// FUNCTION NAME: freelnput() 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Ungrounds the two diff inputs 
// RETURNS: void 
// CALLS: outpw() 

// CALLED BY: none 

void a2dClass : : f reeinput ( void) 

{ 

ctrlw &= -CAL; 
outpw (CNTL, ctrlw) ; 



y ^■k-kir'kir-k-k-k-k-k-k-k-k-k-k-k-k-k'k-k'k'k’k'k'k-k'k-k'k-k'k'k'k'k-k'k-k'k'k-k'k'k'k’k'k'k-k'k-k-k-k-k'k-k'k-k'k'k-k-k-k-k’k-kir-kirir-k 

// FUNCTION NAME: zeroAdjustO 

// AUTHOR: Randy Walker, based on [MAXUS 95] 

// DATE: 27 March 1996 

// DESCRIPTION: Adjust the trimmer on the PGA 
// RETURNS: void 
/ / CALLS : outpw ( ) 

// CALLED BY: none 

y 

void a2dClass ; : zeroAdjust (void) 

{ 

int i; 

unsigned d; 

float sum, of f set Err ; 

clrscr ( ) ; 

printf ( "\n\nADJUST THE TRIM POT FOR 0.0 OFFSET\n\n " ) ; 

setRmsOf f ( ) ; 
setAcDc ( 0 ) ; 
setSequencer ( ) ; 
initTiming (3 ) ; 

while ( ‘ kbhit ( ) ) ( 

set Channel ( 0 , 0 , 3 , 3 ) ; 
grnd Input ( ) ; 
lockTrigger ( ) ; 
resetFif o ( ) ; 
setFifo ( ) ; 
unlockTrigger ( ) ; 
setTrigger ( ) ; 

while (getFifoStatus ( ) i=FULL); 
lockTrigger ( ) ; 

for ( i=0 , sum=0 . 0 ; i<FIFOSIZE ; i++ ) { 
d=getFifoData ( ) ; 
sum+= (float )d*10/2048; 

} 
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offsetErr=( (float) (sum/FIFOSIZE) -10 ) /8000.0; 

printf ("\tTHE MEASURED DC OFFSET IS: %+8 . 6f \r" , of f setErr) ; 

) 

free Input ( ) ; 
getch ( ) ; 



R A2D.CFG 
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7 seqcnt : number_of_seq_addresses_to_load 
;mode_sel : DIFF=1 SE=0 

;mode_acdc :_Signal_coupling_select DC=1 AC = 0 

; Ghent : Number_of_channels_to_sequence_ (hex, _1-F) 

;delta_t : Chan_to_Chan_Sample_rate_in_microsecs_3-8192_=_l/Hz*chcnt 

;samprate: Sample_rate_in_recurrent_mode 0 (fast) -7 (slow) 

; sampindex :_Which_channel_to_sample_in_recurrent_mode 
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APPENDIX B: Serial Port Communications Source Code (C++) 



A. GLOBALS.H 






#ifndef 


__GLOBALS_H 






#define 


_GLOBALS_H 






// types 








typedef 


unsigned char BYTE; 






typedef 


unsigned short WORD; 






typedef 


unsigned long DWORD; 






#def ine 


MEM(seg,ofs) (*((BYTE 


far*)MK_FP(seg,ofs) ) ) 


#def ine 


MEMW(seg, ofs) ( * ( (WORD 


far* )MK_FP (seg, ofs) ) ) 


enum Boolean {FALSE, TRUE} ; 






// basic 


bit twiddles 






#def ine 


set (bit ) 


(l«bit) 


#def ine 


setb (data, bit ) 


data 


1 set (bit) 


#def ine 


clrb(data, bit ) 


data 


Sc Iset(bit) 


#def ine 


setbit (data, bit) 


data 


= setb (data, bit ) 


#def ine 


clrbit (data, bit ) 


data 


= clrb (data, bit ) 


// specific to ports 






#def ine 


setportbit ( reg , bit) 


outportb ( reg, setb ( inportb ( reg ) , bit) ) 


#def ine 


clrportbit (reg, bit) 


outportb(reg, clrb( inportb (reg) ,bit) ) 


#endif 
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B. BUFFER^H 



#ifndef BYTEBUFFER_H 

#define BYTEBUFFER_H 

#include " toetypes .h" 

#define BYTEBUFSIZE 32 

CLASS: Buffer 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION : Base class for use as a polymorphic reference in the serial port code 
which defines a buffer to be used in serial port communications. 

★ ***★★★★*•★****★*★★***■*■****★*■★***★★★★**★****★★★★★**★★★*★★*★**★★**■*•★★★★★★★★★★★ j 



class Buffer { 
public : 

/ /Constructor 

Buffer(WORD sz) : getPtr(O), putPtr(O), size(sz) {) 

//Checks for the arrival of new characters in the buffer 
virtual Boolean hasData ( ) { return Boolean (put Ptr != 

getPtr) ; } 

//How much of the Buffer is used (rounded percentage 
//O - 100) 

virtual int capacityUsed ( ) ; 

//Read from the buffer 

virtual Boolean Get(BYTESc) = 0; 

//Read to the buffer 

virtual void Add (BYTE) = 0; 

protected : 

//Increment the pointer to next position 

void inc(WORDSc index) { if ( + + index == size) index =0; ) 

//Decrement the pointer 

WORD before(WORD index) { return ((index == 0) ? size - 1 : 

index - 1 ) ; } 

WORD get Ptr, //Location of unread data 

put Ptr, //Location to read data to 
size; //Size of the buffer in bytes 
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y********************************************-*--*-********-*-*-*-*-*-****** 

Defines a single buffer of a specified size for buffering charaters 
received via serial port . 

★ *****★★*****★****★*********★★★★★*******★ + *■*•*•*:•*•*•*■*••*•*■*••*•■*■■*•■*■**.*. + *** j 



class byteBuffer : protected Buffer { 
public : 

byteBuffer (BYTE sz=BYTEBUFSIZE) ; 

-byteBuf f er ( ) { delete [] buf; } 

Buffer: rhasData; 

Buffer: : capacityUsed ; 

// buffer extraction 
Boolean Get(BYTE£c); 

// buffer insertion 
void Add (BYTE ch) ; 

void Add(const char*); 

byteBuffer^ operator += (BYTE ch) { Add(ch) ; return *this; 

) 



protected: 

BYTE* buf; 

}; 

#endif 



C. BUFFER.CPP 



#include <iostreain.h> 
#include <stdio.h> 



#include "globals.h 
#include "buffer.h” 



// Buffer 

j y'*************************-*-*********-*--*--*-***-*-** 

// Returns the percentage of the buffer in use 
int 

Buffer: : capacityUsed ( ) 

{ 

int cap = (putPtr + size) % size - getPtr; 
return 10 0 * cap •/ size; 

} 
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// byteBuffer 

j ^★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★* 

//Constructor, instantiates a buffer 
byteBuf f er : :byteBuf f er (BYTE sz) : Buffer(sz) 

{ 

buf = new BYTE[size]; 

} 

//Reads a charcter from the buffer 
Boolean 

byteBuffer: :Get(BYTE& data) 

{ 

if (hasData ( ) ) { 

data = buf[getPtr]; 
inc (getPtr ) ; 
return TRUE; 

} 

return FALSE; 



) 



//Writes a character to the buffer and checks for buffer overflow 
void 

byteBuffer: : Add (BYTE ch) 

{ 

buf[putPtr] = ch; 
inc (putPtr ) ; 

if (IhasData()) { // if there's no data after adding data, 

// it overflowed 

cerr « "\nError: byteBuffer overf low\n" ; 



} 



} 



//Writes a character to the buffer 
void 

byteBuf fer :: Add (const char* s) 

{ 

while (*s) 

Add ( *s++ ) ; 

} 
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D. GPSBUFF.H 



ifndef GPSBUFF_H 

#define GPSBUFF_H 

#include “buffer. h" 

#include "toetypes.h" 

#define GPSBLOCKS 4 

# define LINE_FEED 10 

#define CARR_RETURN 13 

Class buffers GPS position messages via serial port communications. 

Uses a multiple buffer system in which each buffer is capable of holding a 
single position message. Buffers are filled and processed sequentially in a 
round robin fashion. Messages are checked for validity only upon attempted 
reads from the buffer. 

★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★•A- / 



class GPSbuffer ; public Buffer { 
public : 

GPSbuffer (BYTE GPSblocks=GPSBLOCKS ) ; 
-GPSbuf fer () {delete [] block;} 



Boolean hasData(); // a complete structure is ready 

Boolean Get(BYTESc) (return FALSE;} 

Boolean Get (GPSdata) ; // get a complete structure filled in 
void Add (BYTE ch) ; // build the structure as each byte 

// is added 

protected : 

Boolean validHeader (GPSdata ) ; // check a block for valid 

// header 

GPSdata *block; // hold the buffered GPS data 

WORD current, // the current GPS block in use 

last; // the last GPS block in use 



BYTE 

}; 

#endif 



*^put Place; // place to put the next charater received 
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E. GPSBUFF.CPP 



#include <iostream.h> 

#include <stdio.h> 

# include "gpsbuf f .h" 

PROGRAMrGPSbuf f er (Constructor) 

AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Allocates message buffers, indicate that no data has 
beenreceived by equalizing current and last and set position 
into which initial character will be read. 

RETURNS: nothing. 

CALLED BY: navigator class (nav.h) 

CALLS : none . 

*★*★*★*★***★★★**★***★*★*★★★*■*■*■*★★**★*★★*★★★**★*★*★**★***•*■★•*■*★*★★ y/ 



GPSbuf fer : :GPSbuf fer (BYTE GPSblocks) : 
current (0), last(O), 

Buff er (GPSblocks ) // Call to base class constructor 

{ 

block = new GPSdata [GPSblocks ] ; //Create an array of GPSdata 

/ /elements 

putPlace = Sc (block[ current ] [0] ) ; //Set the place for the 

// first character 



} 



PROGRAM iGPSbuffer: ;Add 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Interrupt driven routine which writes incoming characters into the 
gps buffers 
RETURNS .-nothing, 

CALLED BY: interupt driven by buf f eredSerialPort 

CALLS : none . 



void 

GPSbuf fer: : Add {BYTE data) { 

static BYTE lastChar (data) ; //Holds last for <cr> <lf> detection 
static Boolean IfFlag = FALSE; //True when message end is detected 

//Is a new message starting? 
if (IfFlag && (data == '@')) { 

last = current; // Set last to buffer with newest message, 
inc (current ) ; // Set current to the next buffer 

// Set putPlace to the beginning of the next buffer. 
putPlace = & (block[current ] [0] ) ; 

IfFlag = FALSE; // reset for end of next message. 



*putPlace++ = data;// Write character into the buffer. 

//Has the end of a message been received? 
if {(lastChar == CARR_RETURN) && 

(data == LINE_FEED) ) { 

IfFlag = TRUE; 

} 

lastChar = data; //Save last character for <cr> <lf> detection 



} 
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PROGRAMiGPSbuf fer : :Get 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION : Checks to see if a 

new message has arrived, copies it into the 

input argument data and returns a flag to indicate whether a 
newmessage was received. 

RETURNS : TRUE , if a new valid position has been received. 

FALSE, otherwise 
CALLED BY:navPosit (nav.cpp) 

init ializeNavigator (nav.cpp) 

CALLS :GPSbuf fer : :hasData 



Boolean 

GPSbuf f er : :Get (GPSdata data) 

{ 

// Has a new valid message been received, 
if (hasData ( ) ) { 

// Copy the message out of the buffer, 
memcpy (data, block + last, GPSBLOCKSIZE) ; 

// Indicate that this message has been read, 
last = current; 
return TRUE; 

} 

else { 

return FALSE; 

} 

) 
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PROGRAM :GPSbuffer: :hasData 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Determines whether a new message has been received and 
checks to see if it has a valid header. 

RETURNS : TRUE , if a new valid message has been received. 

CALLED BY: GPSbuf f er : : Get {buffer. cpp) 

CALLS: validHeader (buffer. cpp) 



Boolean 

GPSbuf fer : :hasData ( ) 

{ 



// Has a new message with a valid header been received 
if (last != current) { 

if (validHeader (block [last] ) ) { 

return TRUE; 

} 

else { 

return FALSE; 

} 

) 

return FALSE; 

} 

PROGRAM : va 1 idHeader 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION : Checks to see if a message has the proper header for a 
Motorola position message. (@@Ea) 

RETURNS : TRUE, if the header is valid. FALSE, otherwise. 

CALLED BY : GPSbuf fer : :hasData (buffer. cpp) 

CALLS : none . 

COMMENTS : 



Boolean 

GPSbuf fer: : validHeader (GPSdata dataPtr) 

{ 



if ((dataPtr[Oj == '@') && (dataPtr[lJ 

(dataPtr [2] 
(dataPtr [ 3 ] 

return TRUE; 

} 

else { 

return FALSE; 

} 



@- ) 


&& 


E' ) 


Sc Sc 


' a ' 


)) { 
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F. PORTBANK.H 



#ifndef PORTBANK_H 

#define PORTBANK_H 

#include " serial. h" 
#include "buffer.h" 



^★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★*********************** 
CLASS : portBank 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Manages up to four buf f erdSerialPort instances. 



class portBank { 
public: 

portBank ( ) ; 

-portBank () { cleanup (); ) 

buf f eredSer ialPortSc Init(COMport portnum, BYTE irq, 
BaudRate, Pari tyType, BYTE wordlen, BYTE stopbits, 
handshake. Buffers) ; 

void cleanup!); 

friend IntHandlerType COMlhandler, C0M2handler, 
C0M3handler , C0M4handler ; 

protected : 

buf feredSerialPort* ports [4] ; 



}; 



extern portBank COMports ; 
#endi f 



136 



G. PORTBANK.CPP 



#include <iostream. h> 

#include "serial. h" 

# include "buf f er .h" 

# include "portbank.h" 

portBank COMports ; 

// Constructor, sets up array of ports 
portBank: : portBank ( ) 

{ 

for (int i = 0; i < 4; i + +) 
ports [i] = 0; 

} 

// Resets all ports to the original parameters 
void 

portBank : : cleanup ( ) 

{ 

for (int i=0; i<4; i++) 
delete ports [i]; 

} 

// Initializes a serial port based up the input arguments 
buf feredSer ial Port & 

portBank :: Init (COMport portnum, BYTE irq, BaudRate baud, ParityType parity, 
BYTE wordlen, BYTE stopbits, handshake shake. Buffer^ buf) 

{ 

int index = BYTE (portnum) - 1; 
if (ports [ index] ) 

delete ports [ index] ; 

ports [index] = new buf feredSerialPort (portnum, irq, baud, 

parity , wordlen, stopbits, shake, buf ) ; 

return *ports [ index] ; 

} 

// Three specific interrupt handlers which map each interupt to ///the 

proper ISR. 

void 

interrupt 
COMlhandler (...) 

{ 

COMports .ports [ 0 ] ->processInterrupt ( ) ; 

EOI; 

} 

void 

interrupt 
C0M2handler (...) 

{ 

COMports .ports [ 1 ] ->processInterrupt ( ) ; 

EOI; 
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} 



void 

interrupt 

COM3 handler (...) 

{ 

COMports .ports [2 ] ->process Interrupt ( ) ; 
EOI; 

} 

void 

interrupt 
C0M4handler (...) 

{ 

COMports .ports [3 ] ->process Interrupt ( ) ; 
EOI; 

) 



H. SERIAL.H 



#ifndef SERIAL_H 

#define SERIAL_H 

# include <dos.h> 
#include <stdio.h> 
#include "globals.h" 
# include "buf f er .h" 



// user defines 

#define ALMOST_FULL 80 // % full to turn off DTR 

// leave the following alone - hardware specific 



enum 


COMport 


enum 


BaudRate 


enum 


ParityType 


enum 


handshake 


enum 


Shake 


enum 


interruptType 



{C0M1=1, COM2, COM3, COM4 } ; 

{b300, bl200, b2400, b4800, b9600}; 
{ERROR=-l, NOPARITY, ODD, EVEN) ; 

{ NONE , RTS_CTS , XON_XOFF } ; 

{of f , on) ; 

(rx_rdy, tx_rdy, line_stat, modem_stat} 



#def ine 


BIOSMEMSEG 0x40 


#def ine 


DLAB 


0x80 


#def ine 


IRQPORT 


0x21 


#def ine 


EOI 


ou tportb ( 0x2 0 , 0x2 0 ! 


#def ine 


COMlbase 


MEMW (BIOSMEMSEG, 0) 


#def ine 


COM2 base 


MEMW ( BIOSMEMSEG , 2 ) 


#def ine 


COM3base 


0x03e8 


#def ine 


COM4base 


0x02e8 


#def ine 


TX 


(portBase) 


#def ine 


RX 


(portBase ) 


#def ine 


lER 


(portBase+ 1 ) 
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#define HR 
#define LCR 
#define MCR 
#def ine 
#def ine 
#def ine 
#def ine 



(portBase+2 ) 
(portBase+3 ) 
(portBase+4 ) 
(portBase+5) 
(portBase+6 ) 
(portBase) 
(portBase+1) 



LSR 

MSR 

LO_LATCH 

HI_LATCH 



CLASS : serialPort 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Defines a simple serial port. 



class serialPort { 
public : 

serialPort (COMport port, BaudRate, ParityType, BYTE wordlen, 
BYTE stopbits, handshake) ; 



Boolean Send (BYTE data) ; 

Boolean Get (BYTES: data); 



inline Boolean dataReady ( ) ; 
Boolean statusChanged ( ) 



{ returr 


i Boolean (( ifportbit (MSR, 0 ) II ifportbit (MSR, 1 ) ) ) ; } 


// the rest 


are only if handshake 


is specified as RTS_ 


_CTS 


Boolean 


isCTSon ( ) 


{ return ifportbit (MSR, 4 ) ; } 


Boolean 


isDSRon ( ) 


{ return ifportbit (MSR, 5) ; } 


void 


setDTRon ( ) 


{ setportbit (MCR, 0 ) ; 


} 


void 


setDTRoff ( ) 


{ clrportbit (MCR, 0 ) ; 


} 


void 


toggleDTR ( ) ; 






void 


setRTSon ( ) 


{ setportbit (MCR, 1 ) ; 


) 


void 


setRTSof f ( ) 


{ clrportbit (MCR, 1 ) ; 


} 


void 


toggleRTS ( ) ; 






“otected : 








WORD 


portBase; 






handshake 


Shake Type ; 






Shake 


DTRstate, 








RTSstate ; 






inline Boolean i f portbit (WORD, 


BYTE) ; 




inline void 


toggle (Shakes) ; 
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// this is the type for a standard interrupt handler 
typedef void interrupt ( IntHandlerType ) (...); 

CLASS :bufferedSerialPort 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Defines a buffered serial port which is interrupt driven 

on receive, and buffers all incoming characters in the specified buffer 



class buf f eredSerialPort : public serialPort { 
public : 

buf f eredSerialPort (COMport portnum, BYTE irq, BaudRate, 
ParityType, BYTE wordlen, BYTE stopbits, handshake, BufferSc ) ; 

-buf f eredSerialPort ( ) ; 

Boolean Get(BYTESc data); // buffered version 

protected : 

Buffers buf; 

BYTE irqbit ,/ /Value to allow enable PIC interrupts for COM port 
or igirq, / /keep the original value of the 8259 mask register 
comint ; 

void processinterrupt ( ) ; // buffers the incoming character 

IntHandlerType *origcomint ; // keep the original vector for 

//restoring later 

// this allows the actual handlers to access processinterrupt ( ) 
friend IntHandlerType COMlhandler, C0M2handler, C0M3handler, 

C0M4handler ; 



}; 



#endif 
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I. SERIAL.CPP 



#include <iostream.h> 

#include <stdio.h> 

#include "serial. h“ 

Usage Note: Because of the interrupt handlers used, you MUST call your 

buf f eredSerialPort objects portl, port2, or port3, so the 
right handler gets called and can properly service the interrupt. 

★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★■A-** j 



PROGRAM; serialPort (Constructor) 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE; 11 July 1995 

FUNCTION: 

Initializes the one of the Serial Ports. 

1) Determines the base I/O port address for the given COM port 

2) Sets the 8259 IRQ mask value 

3) Initializes the port parameters - baud, parity, etc. 

4) Calls the routine to initialize interrupt handling 

5) Enables DTR and RTS, indicating ready to go 

ArA'ArArA-A-A-ArA'A-A'A-ArA'A-A-A-A-A-ArA-A'A-A-ArA'A-A-A-A-A-A'AArA-A-A-A-A-A-A-ArArArA-ArArA-ArArArArArArArArA-ArArArArArArA- j 

serialPort :: serialPort (COMport port, BaudRate speed, ParityType parity, BYTE 
wordlen, BYTE stopbits, handshake hs) ': 

DTRstate (of f ) , RTSstate (of f ) , ShakeType (hs) 

{ 

switch (port) { 

case COMl : portBase = COMlbase; 

break; 

case COM2: portBase = C0M2base; 

break; 

case COM3: portBase = C0M3base; 

break; 

case COM4: portBase = C0M4base; 

break; 

} //switch 

const WORD bauddiv[] = {0x180, 0x60, 0x30, 0x18, OxC} ; 

// Change 1 

outportb ( lER, 0 ) ; // disable UART interrupts 

(void) inportb (LSR) ; 

(void) inportb (MSR) ; 

(void) inportb (HR) ; 

(void) inportb (RX) ; 

outportb(LCR, DLAB) ; //set DLAB so can set baud rate(read only 

// port) 

outportb (LO_LATCH, bauddiv[ speed] & OxFF); 
outportb (HI_LATCH, (bauddiv [ speed] & OxFFOO) » 8); 
setportbit (MCR, 3 ) ; //turn OUT2 on 

BYTE opt = 0; 
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if (parity != NO PARITY) { 

setbit (opt , 3 ) ; // enable parity 

if (parity == EVEN) // set even parity bit. if odd, leave bit 0 
setbit (opt , 4 ) ; 

) 

// now set the word length, len of 5 sets both bits 0 and 1 to 
// 0, 6 sets to 01, 7 to 10 and 8 to 11 
opt 1= wordlen-5; 
opt 1= --stopbits « 2; 
outportb (LCR, opt ) ; 

if (ShakeType == RTS_CTS) { 
setDTRon ( ) ; 
setRTSon ( ) ; 

} 

} 

PROGRAM: Get 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION .-Gets a byte from the port. Returns true if there's one there, and 
fills in the byte parameter. If theres no character, the parameter is left alone, 
and false is returned 



Boolean 

serialPort : :Get (BYTESc data) 

{ 

if (dataReady ( ) ) { 

data = inportb (RX) ; 
return TRUE; 

} 

else 

return FALSE; 



//make sure there's a char there 
//read character from 8250 
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PROGRAM: Send 

AUTHOR: Frank Kelbe, Eric Bachinann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Outputs a single character to the port. Returns Boolean 
status indicating whether successful 



Boolean 

serialPort :: Send (BYTE data) 

{ 

while ( 1 ( ifportbit (LSR, 5) ) ) // wait until THR ready 

; // NULL statement 

switch (ShakeType) { 
case NONE: 

outportb(TX, data) ; 
return TRUE; 
case RTS_CTS : 

if (isCTSonO Sc& isDSRonO) { 
outportb (TX, data) ; 
return TRUE; 

} 

else return FALSE; 
case XON_XOFF: 
default : 

// add this later if needed 
break; 

) 

return FALSE; 

) 
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PROGRAM; dataReady 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION .-Checks port to see if a character has arrived. 



inline 

Boolean 

ser ialPort : : dataReady { ) 

{ 

/* 

if { ifportbit (LSR, 1 ) ) { 

cerr «"\nOverrun ErrorXn"; 

) 

if { ifportbit (LSR, 2 ) ) { 

cerr «“\nParity ErrorXn"; 

) 

if { ifportbit (LSR, 3 ) ) { 

cerr «"XnFraming ErrorXn"; 

) 

return ifportbit (LSR, 0) ; 



PROGRAM: ifportbit 

AUTHOR; Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: 

•k-k-k-k-k-k*-k-k-k*-k-k-k-k-k-k*-ki(ic-k-k-k-k'k-k-k’k-k-k-k-k-k-k-k'k-k*ic-k-k-k-kicii:ir-k-k-k-k-k-kir-k-k-k-k-kif-k-k-kir j 



inline 

Boolean 

serialPort :: ifportbit (WORD reg, BYTE bit) 

{ 

BYTE on = inportb(reg) ; 
on Sc= set (bit ) ; 

return Boolean (on == set (bit) ); 

} 
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PROGRAM: toggleDTR 

AUTHOR: Frank Kelbe, Eric Bachinann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: toggles Data Transmit Ready if RTS_CTS is off 



void 

serialPort : : toggleDTR { ) 

{ 

if {ShakeType != RTS_CTS) 
return; 

if {DTRstate == off) 
setDTRon { ) ; 
else 

setDTRoff { ) ; 
toggle (DTRstate) ; 



PROGRAM: toggleRTS 

AUTHOR .-Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: toggle Ready to Send (RTS) if RTS_CTS is on. 



void 

serialPort : : toggleRTS ( ) 

{ 

if (ShakeType != RTS_CTS) 
return ; 

if (RTSstate == off) 
setRTSon ( ) ; 
else 

setRTSof f ( ) ; 
toggle (RTSstate) 



j-k'k-k'k'k'k'k’k'k'k’k’k'k'k'k’k’k'k'k’k'k-it'k'if'k-k’k'k’k’k'k-it’k'k'k-k-k-k-k-k’k’k-k-k-if'it'it'if-k'it’k'k-k'k'k’k'if’k'k-k-k’it-k'k 

PROGRAM: toggle 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: toggles value of the input variable 



inline 

void 

serialPort :: toggle ( ShakeS: h) 

{ 

if (h == off) 
h = on ; 
else 

h = off; 

} 
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j l-k-k-k-k-k-k-k-k-k-k-k-if-k-i^-k-k-i^-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k-k 

// buf f eredSerialPort 



/ 






PROGRAM: buf f eredSerialPort (Constructor) 
AUTHOR -.Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 



FUNCTION: 

Initializes 

1 ) 

2 ) 

3 ) 

4) 



the interrupts for the Serial Port, 
takes over the original COM interrupt 
sets the port bits, parity, and stop bit 
enables interrupts on the 8250 (async chip) 
enables the async interrupt on the 8259 PIC 






buf f eredSerialPort : .-buff eredSerialPort (COMport portnum, BYTE irq, 

BaudRate baud, ParityType parity, BYTE wordlen, 
BYTE stopbits, handshake hs, Buffers b) 

: serialPort (portnum, baud, parity, wordlen, stopbits, hs) , 
buf (b) , irqbit ( irq) , comint (irqbit+8) 

{ 

if (ShakeType == RTS_CTS) { // turn it off first, because it 

// was enabled 

setDTRof f ( ) ; // in the base class 

setRTSof f ( ) ; 

} 

origcomint = getvect (comint ) ; //remember the original vector 



switch (portnum) { 
case COMl: 

setvect (comint , COMlhandler) ; 
break; 
case COM2 : 

setvect (comint , COM2handler) ; 
break; 
case COM3 : 

setvect (comint , COM3handler) ; 
break; 
case COM4: 

setvect (comint , COM4handler) ; 
break; 



//point to 



//point to 



//point to 



//point to 



the new handler 



the new handler 



the new handler 



the new handler 



// setportbit (MCR, 3 ) ; 

disable(); // disable all 

setportbit (lER, rx_rdy) ; 
origirq = inportb ( IRQPORT) ; 
clrportbit (IRQPORT, irqbit) ; 



//turn OUT2 on 

interrupts “ critical section 

//enable ints on receive only 
//remember how it was 
//enable COM ints 



if (ShakeType == RTS_CTS) { 
setDTRon ( ) ; 
setRTSon ( ) ; 

} 
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enable ( ) ; 
EOI; 



PROGRAM: -buf f eredSerialPort 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: 

Resets the interrupts. 1) disables the 8250 (async chip) 

2) disables the interrupt chip for async int 

3) resets the 8259 PIC 



buf f eredSerialPort : ; -buf f eredSerialPort ( ) 

{ 

setvect (comint , origcomint ) ; //set the interrupt vector back 
outportb(IER, 0) ; //disable further UART interrupts 

outportb (MCR, 0 ) ; //turn everything off 

outportb ( IRQPORT, origirq) ; 

EOI; 

) 

PROGRAM: Get 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Calls Get base on buffer type 

★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★-A- j 



Boolean 

buf f eredSerialPort : :Get(BYTE& data) 

{ 

return buf . Get (data) ; 

) 

PROGRAM: processinterupt 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Calls the ISR based upon buffer type 

★ ★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★★-A- j 

void 

buf f eredSerialPort : :processInterrupt ( ) 

{ 

if (dataReady ( ) ) { //make sure there's a char there 

BYTE data = inportb(RX) ; //read character from 8250 
buf .Add (data) ; 

if (ShakeType == RTS_CTS && buf . capacityUsed ( ) > ALMOST_FULL) 
setDTRoff 0 ; 

} 

) 



147 



PROGRAM: showPorts 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Prints interupt vector addresses 



int 

showPorts ( ) 

{ 

BYTE* p = (BYTE*)C0M2base; 
p += 5; 

f print f ( stderr , " %X " , *p++ ) ; 

fprintf (stderr, *' %X\n" , *p++ ) ; 
f pr int f ( stderr IRQPORT = %X", inportb ( IRQPORT) ) ; 
return 0 ; 

} 
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J. COMPBUFF.H 



ttifndef COMPBUFF_H 

#define COMPBUFF_H 

#include "buffer.h" 
#include "toetypes.h" 

# define COMPBLOCKS 8 

#define LINE_FEED 10 

#define CARR_RETURN 13 



Class buffers COMPASS messages received via serial port communications. 
Uses a multiple buffer system in which each buffer is capable of 
holding a single message. Buffers are filled and processed 
sequentially in a round robin fashion. Messages are checked for validity 
only upon attempted reads from the buffer. 






class compBuffer : public Buffer { 
public : 



compBuffer (BYTE compBlocks=COMPBLOCKS ) ; 
-compBuff er () {delete [] block;} 



Boolean 

Boolean 

Boolean 

void 



hasData ( ) ; // a complete structure is ready 

Get (BYTES:) (return FALSE;} //Satisfy inheritence 
Get (compData) ; // get a complete structure filled 

Add(BYTE ch) ; // build the structure as each byte 



requirements 

in 

is added 



protected : 



Boolean 


val idHeader ( co: 


compData 


*block; 


// 


WORD 


current , 


// 




last ; 


// 


BYTE 


*put Place; 


// 



npData) ; // check a block for valid header 
Pointer to array of compass messages 
the current comp block in use 
the last comp block in use 

place to put the next charater received 



}; 



#endif 



K. COMPBUFF.CPP 



#include <iostream.h> 

#include <stdio.h> 

# include "compbuff.h" 

PROGRAM: compBuffer (Constructor) 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 28 April 1996 

FUNCTION: 

Allocates message buffers, indicates that no data has been 
received by equalizing current and last and sets the position 
into which initial character will be read. 

RETURNS: nothing. 

CALLED BY: compass class ( compass. h) 

CALLS : none . 



compBuf f er : : compBuf f er (BYTE compBlocks) : 
current (0), last(O), 

Buff er ( compBlocks ) //Call to constructor of the base 

{ 

block = new compDat a [ compBlocks ] ; // Create an array 
putPlace = & (block [current] [0] ) ; // Set position for 

) 



class 

of message buffers 
first character 
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PROGRAM : compBu f f er : : Add 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 28 April 1996 

FUNCTION: 

Interrupt driven routine which writes incoming characters 
into the coompass message buffers 
RETURNS : nothing , 

CALLED BY: interupt driven by buf f eredSerialPort 

CALLS : none . 

************************************************************************* j 



void 

compBuffer :: Add (BYTE data) { 

static Boolean IfFlag = FALSE; //True, if message end detected 
static int messageCount { 0 ) ; // Counts characters in current message 

//Is a new message starting? 
if (IfFlag && (data == '$')) { 

last = current; // Set last to buffer with newest message, 
inc (current) ; // Set current to the next buffer 

// Set putPlace to the beginning of the next buffer. 
putPlace = & (block [ current ][ 0 ]) ; 

IfFlag = FALSE; // reset for end of next message. 

) 

*putPlace++ = data;// Write character into the buffer. 
messageCount++ ; 

//Has the end of a message been received (<crxlf>)? 
if (data == LINE_FEED) { 

IfFlag = TRUE; 

} 
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PROGRAM ; comp Buf fer : :Get 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 28 April 1996 

FUNCTION: 

Checks to see if a new message has arrived, copies it into the 
input argument data and returns a flag to indicate whether a new 
message was received. 

RETURNS : TRUE , if a new valid position has been received. 

FALSE, otherwise 
CALLED BY: compass.cpp 
CALLS : compBuf fer ; :hasData 



Boolean 

compBuf f er : :Get (GPSdata data) 

{ 

// Has a new valid message been received, 
if (hasDataO) { 

// Copy the message out of the buffer, 
memcpy (data, block + last, COMPSIZE) ; 

// Indicate that this message has been read, 
last = current; 
return TRUE; 

} 

else { 

return FALSE; 

) 
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PROGRAM: comp Buff er ; rhasData 
AUTHOR: Eric Bachmann, Randy Walker 
DATE:28 April 1996 
FUNCTION: 

Determines whether a new message has been received and checks 
to see if it has a valid header. 

RETURNS : TRUE , if a new valid message has been received. 

CALLED BY: compBuf f er : :Get 

CALLS: validHeader (compBuf fer . cpp) 

★*★**★★*★***★★**★★**★**★*★*★****★*★****★***★★***★★★★**★★★*★**★**★★★★*★★*★ j 



Boolean 

compBuf fer: :hasData() 

{ 

if ((last i= current) ScSc (validHeader (block [ last ])) ) { 

return TRUE; 

) 

else { 

return FALSE; 

) 

} 

PROGRAM : val idHeader 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 15 May 1996 

FUNCTION: 

Checks to see if a message has the proper header for a compass 
message. ($C) 

RETURNS : TRUE , if the header is valid. FALSE, otherwise. 

CALLED BY : compBuf f er : :hasData 
CALLS : none . 

COMMENTS : 

★ ***★★*★**★****★*★*★★★★*★★★★★★**★★★★*★*★****★***★*★★*★**★★★*★★★★★*★****■** j 



Boolean 

compBuf fer: : validHeader (compData dataPtr) 

{ 

if ((dataPtr[0] == ’$•) && 

(dataPtrfl] == ’C')) { 

return TRUE; 

} 

else { 

return FALSE; 

) 

) 
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